#include "frame.h"
#include "parameterset.h"
#include "utilimagergba.h"
#include "utiltimeprobe.h"
#include "vraibolabmodule.h"
#include "StereoVision.h"
#include <limits>

#define LINE_LUMINANCE 198.0
#define EYEDISTANCE 0.5
#define EPIPOLAR_SCANLINE 128
#define MIN_OBSTACLE_DIST 3.0

AbImageComponent* leftGrayImageComponent = NULL;
AbImageComponent* rightGrayImageComponent = NULL;
AbWindow* leftGrayImgWin = NULL;
AbWindow* rightGrayImgWin = NULL;

bool doStop = false;

// Register this command in the module factory, don't change this!
bool Frame::m_registered = 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
// 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 = NULL;
   
    // CloseObserver is used to shut down the Lab frame
    mp_CloseObserver = NULL;

    //UpdateObserver is used to observe the VRAIBO, to receive synchronized repaints
    mp_UpdateObserver = 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
    // e.g to get the height and width for the used picture
    // (for more information see WISP-Docu: Command, AbstractParameter)

    /*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 = *input1.getValue();
        int width = *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 = 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 = new ParcelIllustrator();
    mp_Illustrator = 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,EYEDISTANCE,-30,-55);


    mp_CloseObserver = new Frame::MyCloseObserver(this);
    mp_UpdateObserver = 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) 
    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()
{
   
    VRAIBO::VRAibo::getInstance()->removeUpdateObserver(mp_Frame->mp_UpdateObserver);
    VRAIBO::VRAibo::getInstance()->removeCloseObserver(this);
    VRAIBOLabModule::setUsed(false);
    if(VRAIBOLabModule::getFactory()->getModule() !=NULL )
    {
        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 = Main::Overseer::getInstance()->getWorkspaceWindow("left image");
    // Create an imageComponent, which can show a picture
    // (for more information see Ab-Docu: AbImageComponent)
    mp_LeftImage = 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 = Main::Overseer::getInstance()->getWorkspaceWindow("right image");
    mp_RightImage = new AbImageComponent();
    mp_RightWindow->setLayoutManager(new AbBorderLayout());
    mp_RightWindow->add(mp_RightImage);
    mp_RightWindow->setResizeable(false);
    mp_RightWindow->setDimension(dim);
    mp_RightWindow->show();

    leftGrayImgWin = Main::Overseer::getInstance()->getWorkspaceWindow("left gray image");
    leftGrayImageComponent = new AbImageComponent();
    leftGrayImgWin->setLayoutManager(new AbBorderLayout());
    leftGrayImgWin->add(leftGrayImageComponent);
    leftGrayImgWin->setResizeable(false);
    leftGrayImgWin->setDimension(dim);
    leftGrayImgWin->show();

    rightGrayImgWin = Main::Overseer::getInstance()->getWorkspaceWindow("right gray image");
    rightGrayImageComponent = new AbImageComponent();
    rightGrayImgWin->setLayoutManager(new AbBorderLayout());
    rightGrayImgWin->add(rightGrayImageComponent);
    rightGrayImgWin->setResizeable(false);
    rightGrayImgWin->setDimension(dim);
    rightGrayImgWin->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 = VRAIBO::VRAibo::getInstance();
}

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


// 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)
{   
	unsigned width = imageLeft.getWidth();
	unsigned height = imageLeft.getHeight();
	std::vector<candidate*> correspPoints;
	BV::ImageRGB leftScanLine(width,1);
	BV::ImageRGB rightScanLine(width,1);
	for(unsigned x=0;x<256;x++) {
		leftScanLine.setPixel(x,0,imageLeft.getPixel(x,EPIPOLAR_SCANLINE));
		rightScanLine.setPixel(x,0,imageRight.getPixel(x,EPIPOLAR_SCANLINE));
	}
	std::vector<BV::RGBPixel> palette;
	BV::RGBPixel red(255,0,0); palette.push_back(red);
	BV::RGBPixel green(0,255,0); palette.push_back(green);
	BV::RGBPixel blue(0,0,255); palette.push_back(blue);
	BV::RGBPixel yellow(255,255,0); palette.push_back(yellow);
	BV::RGBPixel magenta(255,0,255); palette.push_back(magenta);
	BV::RGBPixel cyan(0,255,255); palette.push_back(cyan);
	BV::RGBPixel white(255,255,255); palette.push_back(white);
	BV::RGBPixel black(0,0,0); palette.push_back(black);
	BV::RGBPixel gray(128,128,128); palette.push_back(gray);
	BV::ImageRGB leftImg(imageLeft);
	BV::ImageRGB rightImg(imageRight);
	leftImg.drawLine(BV::Point(0,EPIPOLAR_SCANLINE),BV::Point(width-1,EPIPOLAR_SCANLINE),white);
	rightImg.drawLine(BV::Point(0,EPIPOLAR_SCANLINE),BV::Point(width-1,EPIPOLAR_SCANLINE),white);
	
	correspPoints = StereoVision::findCorrespPointsPMF(leftScanLine,rightScanLine);
	double focalLength = (width/2)/std::tan(((mp_VRAibo->getFovy()*PI)/180)/2);
	unsigned n = 0;
	for(std::vector<candidate*>::iterator i = correspPoints.begin();i!=correspPoints.end();i++) {
		n++;
		leftImg.drawCircle(BV::Point((*i)->xLeft,EPIPOLAR_SCANLINE),8,palette[n]);
		rightImg.drawCircle(BV::Point((*i)->xRight,EPIPOLAR_SCANLINE),8,palette[n]);

		double dispL = (*i)->xLeft+EYEDISTANCE;
		double dispR = (*i)->xRight-EYEDISTANCE;

		Util::Point3d realWorldPoint;
		// X
		realWorldPoint.m_Elements[0] = (-EYEDISTANCE*(dispR+dispL))/(dispR-dispL);
		// Y
		realWorldPoint.m_Elements[1] = 0;
		// Z
		realWorldPoint.m_Elements[2] = std::abs((2*EYEDISTANCE*focalLength)/(dispR-dispL));
		_Console()->appendText(AbString::format("\nReal world Point P[%f , %f , %f]",realWorldPoint.m_Elements[0],realWorldPoint.m_Elements[1],realWorldPoint.m_Elements[2]));
		if(realWorldPoint.m_Elements[2] <= MIN_OBSTACLE_DIST) {
			leftImg.drawText("X",BV::Point((*i)->xLeft-8,EPIPOLAR_SCANLINE-8),palette[n]);
			rightImg.drawText("X",BV::Point((*i)->xRight-8,EPIPOLAR_SCANLINE-8),palette[n]);
			doStop = true;
		}
	}
	//_Console()->appendText(AbString::format("\nFound %d corresponding edge point in scanline",correspPoints.size()));
	leftGrayImageComponent->setNewImage(leftImg.getBufferPointer(),leftImg.getWidth(),leftImg.getHeight());
	rightGrayImageComponent->setNewImage(rightImg.getBufferPointer(),rightImg.getWidth(),rightImg.getHeight());	
	
	/*
	BV::ImageRGB farbbildHSV; 
    BV::Filter2D::ColorSpaceTransformationFilters::filterConvertRGB2HSV(imageLeft, farbbildHSV);
	BV::Image helligkeitbild;
	BV::Filter2D::ImageFeatureExtractionFilters::filterExtractComponent(farbbildHSV, helligkeitbild, 2);
	int h =  (int) (height * 0.75);
	BV::RGBPixel test;
	bool begin = true;
	unsigned start = 0;
	unsigned end = 0;
	double linePosRight = -1.0;
	double linePosLeft = -1.0;
	double lum = -1.0;
	for(unsigned x = 0; x < width; x++)
	{
		//TB: only for debug issues, the function call could be direct in the if below
		test = helligkeitbild.getPixel(x,h);
		//TB: get the right side of the red line
		lum = test.getLuminance();
		if( (2.0 >= std::abs(lum - LINE_LUMINANCE))&& begin == true)
		{
			start = x;
			begin = false;
		}
		//TB: get the left side if the red line
		else if( ( 2.0 < std::abs(lum - LINE_LUMINANCE) || x == width-1) && begin == false)
		{
			//TB: calc the middle of the red line and return this point
			end = x-1;
			//linePos = (start + end)*0.5;
			//linePos = start + ((end - start)*0.5);
			linePosLeft = start + ((end - start)*0.5);
		}
		
	}
	BV::Filter2D::ColorSpaceTransformationFilters::filterConvertRGB2HSV(imageRight, farbbildHSV);
	BV::Filter2D::ImageFeatureExtractionFilters::filterExtractComponent(farbbildHSV, helligkeitbild, 2);
	begin = true;
	for(unsigned x = 0; x < width; x++)
	{
		//TB: only for debug issues, the function call could be direct in the if below
		test = helligkeitbild.getPixel(x,h);
		//TB: get the right side of the red line
		lum = test.getLuminance();
		if( (2.0 >= std::abs(lum - LINE_LUMINANCE) )&& begin == true)
		{
			start = x;
			begin = false;
		}
		//TB: get the left side if the red line
		else if( ( 2.0 < std::abs(lum - LINE_LUMINANCE) || x == width-1) && begin == false)
		{
			//TB: calc the middle of the red line and return this point
			end = x-1;
			//linePos = (start + end)*0.5;
			//linePos = start + ((end - start)*0.5);
			linePosRight = start + ((end - start)*0.5);
		}
		
	}
	if( (linePosLeft == -1.0) || (linePosRight == -1.0) ) {
		return false;
	} else {
		// cyclop view...
		linePos = (linePosLeft+linePosRight)/2;
		return true;
	}
	*/
    return false;
}


// 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
    unsigned char* tempLeft = mp_VRAibo->getLeftImage();
    unsigned char* tempRight = mp_VRAibo->getRightImage();
   
    if(tempLeft == NULL || tempRight == NULL)
    {
        return;
    }

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

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

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

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


    if (evaluate(imageLeft,imageRight,linePos))//pos found
    {
        // Point of interest has been found
        //TB: calc the angle for the rotation of the robo (see worksheet)
        walkAngle = (mp_VRAibo->getFovy()/mp_VRAibo->getPictureWidth())*-(linePos-imageLeft.getWidth()*0.5);
        // 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));
        _Console()->appendText(AbString::format("aktueller Walkangle: %d\n", walkAngle));

        //let the VRAIBO turn "walkAngle" degrees
        mp_VRAibo->turn(walkAngle);
        //let the VRAIBO walk 0.5
        mp_VRAibo->walk(0.5);
       
    }
    else
    {
        //TB: if no line was found walk forward...
		if(!doStop) {
			mp_VRAibo->walk(0.5);
		} else {
			_Console()->appendText("\nObstacle in front detected");
			doStop = false;
		}
		
        // Point of interest has not been found
        // Search for point of interest
        _Console()->appendText("\nNo Lineposition found");
    }
}