//-*-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(blue_index);
        req.objectColors[ellipseDataType].insert(pink_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))); 

        //name is deceiving, longH and secLongH returns thus that longH is the longest top line and secLongH is the longest lowest line. 

         //find 2 longest horizontal lines

         NEW_SHAPE(longH, LineData, LineData::LineData( hlines.at(0))); 
         NEW_SHAPE(secLongH, LineData, LineData::LineData(hlines.at(1))); 
     
         coordinate_t highest =( longH->leftPt() ).coordY(); 
         coordinate_t lowest = (secLongH->leftPt()).coordY(); 
        
        //decide for the first two lines who's longer
        if((hlines.at(0))->getLength() < (hlines.at(1))->getLength())
        {
            longH = hlines.at(1);
            secLongH = hlines.at(0); 
        }
    
        //decide highest and lowest
        if ( lowest > highest)
        {
            coordinate_t tmp = highest;
            highest = lowest; 
            lowest = tmp; 
        }
    
        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())
            {
                coordinate_t tmp = (hlines.at(i)->leftPt()).coordY(); 
                //check whether bigger than longest
                if(l > longH->getLength())
                {
                    secLongH = longH; 
                    longH = hlines.at(i); 
                }
                else
                {
                    secLongH = hlines.at(i);
                }
    
                if(tmp > highest) 
                {
                    highest = tmp;
                }
                else if (tmp < lowest) 
                {
                    lowest = tmp; 
                } 
            }
        }


        //find 2 longest vertical lines
        NEW_SHAPE(longV, LineData, LineData::LineData( vlines.at(0))); 
        NEW_SHAPE(secLongV, LineData, LineData::LineData(vlines.at(1))); 
       
       
        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