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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"
#include "Shared/mathutils.h"
#include <string>

using namespace DualCoding;
using namespace mathutils; 

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()));
      
        //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_SHAPEVEC(hlinesSorted, LineData, stable_sort(hlines, LineData::LengthLessThan()));
    
        int longH = hlinesSorted.size()-1; 
        int secLongH =1; 
        int x = hlinesSorted.size()-2; 

        while (x!=-1) 
        {
            if(LineData::ColinearTest()(hlinesSorted.at(longH), hlinesSorted.at(x)))
            {
                x--; 
            }
            else
            {
                //find index of the 2nd longest one
                secLongH = x; 
                x = -1 ; 
            }
        }

        NEW_SHAPE(l1, LineData, (hlinesSorted.at(longH)));
        NEW_SHAPE(l2, LineData, (hlinesSorted.at(secLongH))); 
        
        int topInt = 0; 
        int botInt = 0; 

        if((hlinesSorted.at(longH)->leftPt()).coordY() > (hlinesSorted.at(secLongH)->leftPt()).coordY())
        {   
            topInt = secLongH; 
            botInt = longH; 
        }
        else
        {
            topInt = longH; 
            botInt = secLongH; 
        }

        NEW_SHAPE(top, LineData, hlinesSorted.at(topInt)); 
        NEW_SHAPE(bot, LineData, hlinesSorted.at(botInt)); 
       
        AngPi orientH1 = top -> getOrientation(); 
        AngPi orientH2 = bot -> getOrientation(); 

 
        //do the same thing for vertical lines  
         NEW_SHAPEVEC(vlinesSorted, LineData, stable_sort(vlines, LineData::LengthLessThan()));
    
        int longV = vlinesSorted.size()-1; 
        int secLongV =1; 
        int y = vlinesSorted.size()-2; 

        while (y!=-1) 
        {
            if(LineData::ColinearTest()(vlinesSorted.at(longV), vlinesSorted.at(y)))
            {
                y--; 
            }
            else
            {
                //find index of the 2nd longest one
                secLongV = y; 
                y = -1 ; 
            }
        }

        NEW_SHAPE(v1, LineData,(vlinesSorted.at(longV)));
        NEW_SHAPE(v2, LineData,(vlinesSorted.at(secLongV))); 

        printf("long1: x: %d, secone :x %d", ( vlinesSorted.at(longV)->leftPt()).coordX(), (vlinesSorted.at(secLongV)->leftPt()).coordX());
       
       int leftInt=0; 
       int rightInt = 0; 
        if((vlinesSorted.at(longV)->leftPt()).coordX() > (vlinesSorted.at(secLongV)->leftPt()).coordX())
        {   
            leftInt = secLongV; 
            rightInt = longV; 
        }
        else
        {
            leftInt = longV; 
            rightInt = secLongV; 
        }

        NEW_SHAPE(leftV, LineData, vlinesSorted.at(leftInt)); 
        NEW_SHAPE(rightV, LineData, vlinesSorted.at(rightInt)); 

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


        EndPoint mostRight=(hlines.at(0))->rightPt(); 
        //find the right most point on the board
        for (unsigned int i = 1; i<hlines.size(); i++)
        {
            mostRight = rightMost(mostRight, (hlines.at(i))->rightPt()); 
        }
        
        EndPoint mostTop=(vlines.at(0))->topPt(); 
        //find the top most point on the board
        for (unsigned int i = 1; i<vlines.size(); i++)
        {
            mostTop = topMost(mostTop, (vlines.at(i))->topPt()); 
        }
        
        EndPoint mostBottom=(vlines.at(0))->bottomPt(); 
        //find the bottom most point on the board
        for (unsigned int i = 1; i<vlines.size(); i++)
        {
            mostBottom = bottomMost(mostBottom, (vlines.at(i))->leftPt()); 
        }
        
        AngPi orientV1 = (leftV)->getOrientation();
        AngPi orientV2 = (rightV)->getOrientation();
        
        // 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); 

        // Top Border
        NEW_SHAPE(tborder, LineData, LineData::LineData(camShS, mostTop, orientH1)); 
        tborder->setInfinite(true);

        // Bottom Border
        NEW_SHAPE(bborder, LineData, LineData::LineData(camShS, mostBottom, orientH2));
        bborder->setInfinite(true); 
       
        // Basic Edges
        NEW_SKETCH(leftthree, bool, visops::leftHalfPlane(leftV));
        NEW_SKETCH(rightthree, bool, visops::rightHalfPlane(rightV));
        NEW_SKETCH(leftsix, bool, visops::leftHalfPlane(rightV));
        NEW_SKETCH(botthree, bool, visops::bottomHalfPlane(bot));
        NEW_SKETCH(topthree, bool, visops::topHalfPlane(top));
        NEW_SKETCH(topsix, bool, visops::topHalfPlane(bot));

        // Borders
        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));
        
        // Everything outside the grid
        NEW_SKETCH(outside, bool, (leftborder | rightborder | topborder | botborder));

        // Everything inside the grid
        NEW_SKETCH(inside, bool, !outside);

        // Columns
        NEW_SKETCH(boardleftthree, bool, (leftthree & inside));
        NEW_SKETCH(boardrightthree, bool, (rightthree & inside));
        NEW_SKETCH(boardvmiddlethree, bool, ((leftsix ^ leftthree) & inside));

        // Rows
        NEW_SKETCH(boardtopthree, bool, (topthree & inside));
        NEW_SKETCH(boardbotthree, bool, (botthree & inside));
        NEW_SKETCH(boardhmiddlethree, bool, ((topsix ^ topthree) & inside));

        // Individual Squares
        // 1 | 2 | 3
        // 4 | 5 | 6
        // 7 | 8 | 9
        NEW_SKETCH(b1, bool, (boardleftthree & boardtopthree));
        NEW_SKETCH(b2, bool, (boardvmiddlethree & boardtopthree));
        NEW_SKETCH(b3, bool, (boardrightthree & boardtopthree));
        NEW_SKETCH(b4, bool, (boardleftthree & boardhmiddlethree));
        NEW_SKETCH(b5, bool, (boardvmiddlethree & boardhmiddlethree));
        NEW_SKETCH(b6, bool, (boardrightthree & boardhmiddlethree));
        NEW_SKETCH(b7, bool, (boardleftthree & boardbotthree));
        NEW_SKETCH(b8, bool, (boardvmiddlethree & boardbotthree));
        NEW_SKETCH(b9, bool, (boardrightthree & boardbotthree));

      }
};

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

#endif