//-*-c++-*-
#ifndef INCLUDED_ParseBoard_h_
#define INCLUDED_ParseBoard_h_

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"
#include <string>
#
using namespace DualCoding;

class BuildMap : public VisualRoutinesStateNode {
public:
  BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
  virtual void DoStart() {
    
    //**************** Set up MapBuilder request ****************
		const int pink_index = ProjectInterface::getColorIndex("pink");
    const int orange_index = ProjectInterface::getColorIndex("orange");
    const int green_index = ProjectInterface::getColorIndex("green");
    const int yellow_index = ProjectInterface::getColorIndex("yellow");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    NEW_SHAPE(gazePt, PointData, new PointData(camShS,Point(400,0,-100,egocentric)));

    MapBuilderRequest req(MapBuilderRequest::cameraMap);
   req.maxDist=1200;
   req.immediateRequest = true; 
    

    req.objectColors[lineDataType].insert(yellow_index);

    req.occluderColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(pink_index);
    req.occluderColors[lineDataType].insert(green_index);
    req.occluderColors[lineDataType].insert(orange_index);

   // req.objectColors[ellipseDataType].insert(orange_index);
    req.objectColors[ellipseDataType].insert(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);
   // req.objectColors[ellipseDataType].insert(green_index);

    mapBuilder.executeRequest(this,req);
  }
};

class ExamineMap : public VisualRoutinesStateNode {
    public:
	ExamineMap() : VisualRoutinesStateNode("ExamineMap") {}

    virtual void DoStart() {
    
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());
   
    //Get masked blobs
    NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame, "pink"));
    NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));
    NEW_SKETCH(yellow_stuff, bool, visops::colormask(camFrame, "yellow"));
    
    //Extract yellow lines
    NEW_SHAPEVEC(ylines, LineData, LineData::extractLines(yellow_stuff));
    
    //find horizontal lines
    NEW_SHAPEVEC(hlines, LineData, subset(ylines,LineData::IsHorizontal()));
    
    //NEW_SHAPE(longestH, LineData, max_element(hlines, LineData::LengthLessThan()));
    
    // Find vertical lines
    NEW_SHAPEVEC(vlines, LineData, subset(ylines, LineData::IsVertical(M_PI/6)));
    
    //NEW_SHAPE(longestV, LineData, max_element(vlines, LineData::LengthLessThan()));
  
//TODO delete this later
    NEW_SHAPE(v1, LineData, LineData::LineData(vlines.at(0)));
    NEW_SHAPE(v2, LineData, LineData::LineData(vlines.at(1))); 



//find 2 longest horizontal lines

     NEW_SHAPE(longH, LineData, LineData::LineData( hlines.at(0))); 
     NEW_SHAPE(secLongH, LineData, LineData::LineData(hlines.at(1))); 
 
    
    for (unsigned int i = 1; i< hlines.size() ; i++)
    {
        //if they are the same line
        if(i==1)
        {
            if(ColinearTest(LongH, secLongH)
        }
    }

        if((hlines.at(0))->getLength() < (hlines.at(1))->getLength())
    {
        longH = hlines.at(1);
        secLongH = hlines.at(0); 
    }
    
    for(unsigned int i=2; i<hlines.size(); i++)
    {
        int l = (hlines.at(i))->getLength() ; 
        if( l < secLongH->getLength())
        {
            continue; 
        }
        else if (l > secLongH->getLength())
        {
            //check whether bigger than longest
            if(l > longH->getLength())
            {
                secLongH = longH; 
                longH = hlines.at(i); 
            }
            else
            {
                secLongH = hlines.at(i);
            }
        }
    }


 //find 2 longest vertical lines
     NEW_SHAPE(longV, LineData, LineData::LineData( vlines.at(0))); 
     NEW_SHAPE(secLongV, LineData, LineData::LineData(vlines.at(1))); 
   
    coordinate_t highest =( longV->leftPt() )->coordY(); 
    coordinate_t lowest; 
    
    if((vlines.at(0))->getLength() < (vlines.at(1))->getLength())
    {
        longV = vlines.at(1); 
        secLongV = vlines.at(0); 
    }

    
    for(unsigned int i=2; i<vlines.size(); i++)
    {
        int l = (vlines.at(i))->getLength() ; 
        if( l < secLongV->getLength())
        {
            continue; 
        }
        else if (l > secLongV->getLength())
        {
            //check whether bigger than longest
            if(l > longV->getLength())
            {
                secLongV = longV; 
                longV = vlines.at(i); 
            }
            else
            {
                secLongV= vlines.at(i);
            }
        }
    }


    //find the left most point 
    EndPoint mostLeft = leftMost((hlines.at(0))->leftPt(), (hlines.at(1))->leftPt());
    
    for(unsigned int i=2; i<hlines.size(); i++)
    {
        mostLeft = leftMost((hlines.at(i))->leftPt(), mostLeft);
    }

    //find the right most point
    EndPoint mostRight = rightMost((hlines.at(0))->rightPt(), (hlines.at(1))->rightPt());
    
    for(unsigned int i=2; i<hlines.size(); i++)
    {
        mostRight = rightMost((hlines.at(i))->rightPt(), mostRight);
    }
    EndPoint l1 = longH->leftPt(); 
    EndPoint r1 = longH->rightPt(); 
    EndPoint l2 = secLongH->leftPt();
    EndPoint r2 = secLongH->rightPt(); 
    EndPoint t1 = (vlines.at(0))->topPt(); 
    EndPoint b1 = (vlines.at(0))->bottomPt();
    EndPoint t2 = (vlines.at(1)) -> topPt();
    EndPoint b2 = (vlines.at(1))-> bottomPt(); 

    

    AngPi orientV1 = (vlines.at(0))->getOrientation();
    AngPi orientV2 = (vlines.at(1))->getOrientation();
    AngPi orientH1 = longH -> getOrientation(); 
    AngPi orientH2 = secLongH -> getOrientation(); 

    // Top Border
    NEW_SHAPE(tborder, LineData, LineData::LineData(camShS, longV->topPt(), orientH2)); 
    tborder->setInfinite(true);

    // Bottom Border
    NEW_SHAPE(bborder, LineData, LineData::LineData(camShS, b1, b2));
    bborder->setInfinite(true); 

    // Left Border
    NEW_SHAPE(lborder, LineData, LineData::LineData(camShS, mostLeft, orientV1));
    lborder->setInfinite(true);
  
    // Right Border
    NEW_SHAPE(rborder, LineData, LineData::LineData(camShS,mostRight, orientV2));
   rborder->setInfinite(true); 

    EndPoint h1 = longH->topPt(); 
    EndPoint h2 = secLongH->topPt(); 

        NEW_SHAPE(topLine, LineData, LineData::LineData(secLongH));
        NEW_SHAPE(secTopLine, LineData, LineData::LineData(longH));
    
    if(longH->pointOnLine(topMost(h1, h2)))
    {
        topLine=longH;
        secTopLine = secLongH;
    }


    NEW_SKETCH(leftthree, bool, visops::leftHalfPlane(vlines.at(0)));
    NEW_SKETCH(rightthree, bool, visops::rightHalfPlane(vlines.at(1)));
    NEW_SKETCH(leftsix, bool, visops::leftHalfPlane(vlines.at(1)));
    NEW_SKETCH(leftborder, bool, visops::leftHalfPlane(lborder));
    NEW_SKETCH(rightborder, bool, visops::rightHalfPlane(rborder));
    NEW_SKETCH(topborder, bool, visops::topHalfPlane(tborder));
    NEW_SKETCH(botborder, bool, visops::bottomHalfPlane(bborder));
    
    NEW_SKETCH(outside, bool, (leftborder | rightborder | topborder | botborder));
    NEW_SKETCH(inside, bool, !outside);

    NEW_SKETCH(boardleftthree, bool, (leftthree & inside));

    NEW_SKETCH(borardrightthree, bool, (rightthree & inside));

    NEW_SKETCH(boardmiddlethree, bool, ((leftsix ^ leftthree) & inside));


  }
};

    class ParseBoard : public VisualRoutinesStateNode {
        public:
        ParseBoard(): VisualRoutinesStateNode("ParseBoard") {}
      
        virtual void setup() {
            #statemachine
            startnode: BuildMap() =MAP=> ExamineMap()
            #endstatemachine
        }
};

#endif