#include "frame.h"
#include "parameterset.h"
#include "utilimagergba.h"
#include "utiltimeprobe.h"
#include "vraibolabmodule.h"

// Register this command in the module factory, don't change this!
bool Frame::m_registered =3D =
VRAIBOLabModule::getFactory()->registerPrimaryCommand(&create);

// Constructor of this command.
// The first parameter is the used CommandDescriptor, which will =
identify this Command.
// The second parameter is the used CommandGUIDescriptor, which will =
specify the menue and the=20
// displayed text. (for more information see WISP-Docu: =
CommandDescriptor, CommandGUIDescriptor)
Frame::Frame():Command(new Main::CommandDescriptor("show the =
parcour-OpenGLscene","vrAiboFrame"),
					   new =
Main::CommandGUIDescriptor(Main::CommandGUIDescriptor::ON_MENU,"VRAIBO =
Frame", "Show Scene Parcour"))
{
	// Illustrator is used to show the OPENGL scene
	mp_Illustrator =3D NULL;
=09
	// CloseObserver is used to shut down the Lab frame
	mp_CloseObserver =3D NULL;

	//UpdateObserver is used to observe the VRAIBO, to receive synchronized =
repaints
	mp_UpdateObserver =3D NULL;
}

// Destructor of the class
Frame::~Frame()
{

}

// This method will be executed if the Command was called from the WISP =
console or WISP GUI.
void Frame::execute()
{

	// if you want to use input parameters, you can do it like the =
following lines=20
	// e.g to get the height and width for the used picture
	// (for more information see WISP-Docu: Command, AbstractParameter)=20

	/*Main::Parameter<int> input1;
	Main::Parameter<int> input2;
	if(!mp_Selected->getParameterList()->empty())
	{
		//solver solves casting problems
		if(!m_Solver.solve(&input1,mp_Selected->getParameterList()->front()))
		{
			=
Main::Overseer::getInstance()->getOutputConsole()->appendText("Parameter =
failure");
			return;
		}

		if(!m_Solver.solve(&input2,mp_Selected->getParameterList()->at(1)))
		{
			=
Main::Overseer::getInstance()->getOutputConsole()->appendText("Parameter =
failure");
			return;
		}
		int height =3D *input1.getValue();
		int width =3D *input2.getValue();
	}*/

	// The VRAibo class is the central interface to the virtual AIBO =
(neccessary to let the VRAIBO walk for example)
	// (for more information see WISP-Docu: VRAibo)
	VRAIBO::VRAibo* vraibo =3D VRAIBO::VRAibo::getInstance();

	if(vraibo->isRunning())
	{
		// Example to use the console for outputs with a linebreak "\n".
		// alternative, you can use =
Main::Overseer::getInstance()->getOutputConsole()->appendText("");
		_Console()->appendText("already running \n");
		return;
	}
	// Illustrator is used to show the OPENGL scene for example the =
Parcourillustrator or Parcelillustrator
	//mp_Illustrator =3D new ParcelIllustrator();
	mp_Illustrator =3D new ParcourIllustrator(); //->alternative =
illustrator

	// activate the viewing of the opengl scenes ( four different camera =
positions )
	// params are the used illustrator, height, width, eye distance to =
center of head
	// (half distance between both eyes), start PositonX, startPositionY
	// (for more information see WISP-Docu: VRAibo)
	//vraibo->showViews(mp_Illustrator,256,256,0.5,-30,-55);
	vraibo->showViews(mp_Illustrator,256,256,0.0,-30,-55);


	mp_CloseObserver =3D new Frame::MyCloseObserver(this);
	mp_UpdateObserver =3D new Frame::MyUpdateObserver(this,256,256);

	//add observers to VRAIBO
	vraibo->addCloseObserver(mp_CloseObserver);
	vraibo->addUpdateObserver(mp_UpdateObserver);

	_Console()->appendText("ready\n");

	// Inform module about it's use (ensure correct unloading of the =
module) =20
	VRAIBOLabModule::setUsed(true);
}

// Creation method for this Command
Main::Command* Frame::create()
{
	return new Frame();
}


// Constructor of MyCloseObserver
Frame::MyCloseObserver::MyCloseObserver(Frame* scenes)
:mp_Frame(scenes)
{

}

// this method is used to shut down the Lab frame
void Frame::MyCloseObserver::update()
{
=09
	=
VRAIBO::VRAibo::getInstance()->removeUpdateObserver(mp_Frame->mp_UpdateOb=
server);
	VRAIBO::VRAibo::getInstance()->removeCloseObserver(this);
	VRAIBOLabModule::setUsed(false);
	if(VRAIBOLabModule::getFactory()->getModule() !=3DNULL )
	{
		delete mp_Frame->mp_Illustrator;
		delete mp_Frame->mp_UpdateObserver;
	}
	delete this;
}

// Constructor of MyUpdateObserver
Frame::MyUpdateObserver::MyUpdateObserver(Frame* scenes,int =
imageWidth,int imageHeight)
:mp_Frame(scenes)
{
	// Get a workspacewindow from the Overseer to put elements on it, like =
pictures, buttons, labels etc..
 	// (for more information see Ab-Docu: AbWindow, WISP-Docu: Overseer)
	mp_LeftWindow =3D =
Main::Overseer::getInstance()->getWorkspaceWindow("left image");
	// Create an imageComponent, which can show a picture
	// (for more information see Ab-Docu: AbImageComponent)
	mp_LeftImage =3D new AbImageComponent();
	// Set the layout for the window (usefull to arrange components)
	// (for more information see Ab-Docu: AbBorderLayout)
	mp_LeftWindow->setLayoutManager(new AbBorderLayout());
	// Add the imageComponent to the window
	mp_LeftWindow->add(mp_LeftImage);
	// Set the dimensio of the window
	AbDimension dim(0,0,imageWidth,imageHeight);
	mp_LeftWindow->setDimension(dim);
	// disable resizing of the window
	mp_LeftWindow->setResizeable(false);
	// Show the window
	mp_LeftWindow->show();


	mp_RightWindow =3D =
Main::Overseer::getInstance()->getWorkspaceWindow("right image");
	mp_RightImage =3D new AbImageComponent();
	mp_RightWindow->setLayoutManager(new AbBorderLayout());
	mp_RightWindow->add(mp_RightImage);
	mp_RightWindow->setResizeable(false);
	mp_RightWindow->setDimension(dim);
	mp_RightWindow->show();

	// The VRAibo class is the central interface to the virtual AIBO =
(neccessary to let the VRAIBO walk for example)
	// (for more information see WISP-Docu: VRAibo)
	mp_VRAibo =3D VRAIBO::VRAibo::getInstance();

}

// destructor of MyUpdateObserver
Frame::MyUpdateObserver::~MyUpdateObserver()
{
	//close all own windows
	AbMemoryManaged::destroy(mp_LeftWindow);
	AbMemoryManaged::destroy(mp_RightWindow);
}


// This method will return true, if a point of interest has been found.
// image Left/Right is the picture of the actual position of VRAIBO. You =
can use this to analyse
// your situation with different image algorithms or something else.
// linePos is the Pixel, which will be used to calculate the next angle =
of rotation
// (e.g. the center of the line, which you want to follow).
bool Frame::MyUpdateObserver::evaluate(BV::ImageRGB& =
imageLeft,BV::ImageRGB& imageRight, double& linePos)
{=09
	int x, start, end, h, w;
	BV::RGBPixel red(198, 0, 0);
	BV::RGBPixel test;
	bool begin =3D true;
//	double terror;

	//heigt, the line is in the lower quarter of the left image
	h =3D (int) (imageLeft.getHeight()*0.75);
	//width
	w =3D imageLeft.getWidth();
	for(x =3D 0; x < w; x++)
	{
		//TB: only for debug issues, the function call could be direct in the =
if below
		test =3D imageLeft.getPixel(x,h);
		//terror =3D imageLeft.getPixel(x,h).getScalarValue();
		//_Console()->appendText(AbString::format("DEBUG Scalar: %f\n",  =
terror));
		//_Console()->appendText(AbString::format("DEBUG RGB: %f %f %f \n", =
imageLeft.getPixel(x,h).getRed(), imageLeft.getPixel(x,h).getGreen(), =
imageLeft.getPixel(x,h).getBlue() ));


		//TB: get the right side of the red line
		if(test =3D=3D red && begin =3D=3D true)
		{
			start =3D x;
			begin =3D false;
		}
		//TB: get the left side if the red line
		else if(imageLeft.getPixel(x,h) !=3D red && begin =3D=3D false)
		{
			//TB: calc the middle of the red line and return this point
			end =3D x-1;
			linePos =3D (start + end)*0.5;
			return true;
		}
	=09
	}



	return false;
=09
}


// This method will be invoked if the VRAIBO has synchronized all views =
again
void Frame::MyUpdateObserver::update()
{

	// Get a rgb-images of VRAIBO's camera=20
	unsigned char* tempLeft =3D mp_VRAibo->getLeftImage();
	unsigned char* tempRight =3D mp_VRAibo->getRightImage();
=09
	if(tempLeft =3D=3D NULL || tempRight =3D=3D NULL)
	{
		return;
	}

	// Set image for the imageComponents, which shows the Images
	if(mp_LeftWindow !=3D NULL)
	{
		mp_LeftImage->setNewImage(tempLeft, mp_VRAibo->getPictureWidth(), =
mp_VRAibo->getPictureHeight(), Ab::RGB24);
	}

	if(mp_RightWindow !=3D NULL)
	{
		mp_RightImage->setNewImage(tempRight, mp_VRAibo->getPictureWidth(), =
mp_VRAibo->getPictureHeight(), Ab::RGB24);
	}

=09
	// Create a ImageRGBs for image processing and set buffer pointer for =
bvImages
	BV::ImageRGB imageLeft;
	=
imageLeft.setBufferPointer(mp_VRAibo->getPictureWidth(),mp_VRAibo->getPic=
tureHeight(),tempLeft);
=09
	BV::ImageRGB imageRight;
	=
imageRight.setBufferPointer(mp_VRAibo->getPictureWidth(),mp_VRAibo->getPi=
ctureHeight(),tempRight);

	// Set the evaluation return value "linePos" to a default value
	double linePos =3D 0;
	// Set the angle of rotation for AIBO to a default value
	double walkAngle =3D 0;


	if (evaluate(imageLeft,imageRight,linePos))//pos found
	{
		// Point of interest has been found

		// This is a example for a console output in which a variable
		// and a string will be written to the console
		// AbString::format() is used like printf()
		// (for more information see Ab-Docu: AbString)
		_Console()->appendText(AbString::format("aktuelle Linienposition: =
%f\n", linePos));
	=09
		//TB: calc the angle for the rotation of the robo (see worksheet)
		walkAngle =3D =
(mp_VRAibo->getFovy()/mp_VRAibo->getPictureWidth())*-(linePos-imageLeft.g=
etWidth()*0.5);
	=09
		_Console()->appendText(AbString::format("aktueller Walkangle: %d\n", =
walkAngle));

		//let the VRAIBO turn "walkAngle" degrees=20
		mp_VRAibo->turn(walkAngle);
		//let the VRAIBO walk 0.5=20
		mp_VRAibo->walk(0.5);
	=09
	}
	else
	{
		//TB: if no line was found walk forward...
		mp_VRAibo->walk(0.5);
		// Point of interest has not been found
		// Search for point of interest
		_Console()->appendText("No Lineposition found \n");
	}

=09
}