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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"

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.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() {
    const int pink_index = ProjectInterface::getColorIndex("pink");
    const int blue_index = ProjectInterface::getColorIndex("blue");
    
    cout << "MapBuilder found " << camShS.allShapes().size() << " shapes." << endl;
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS))
    lines = stable_sort(lines, not2(LineData::LengthLessThan()));

    NEW_SHAPEVEC(horiz, LineData, subset(lines,LineData::IsHorizontal()))
    NEW_SHAPEVEC(vert, LineData, subset(lines,not1(LineData::IsHorizontal())))
    
    horiz.resize(2);
    vert.resize(2);

    horiz = stable_sort(horiz, IsAbove());
    vert = stable_sort(vert, IsLeftOf());

    NEW_SHAPE(leftBorder, LineData, new LineData(camShS, leftMost(horiz[0]->leftPt(), horiz[1]->leftPt()), vert[0]->getOrientation()))
    
    NEW_SHAPE(rtBorder, LineData, new LineData(camShS, rightMost(horiz[0]->rightPt(), horiz[1]->rightPt()), vert[1]->getOrientation()))

    NEW_SHAPE(topBorder, LineData, new LineData(camShS, topMost(vert[0]->topPt(), vert[1]->topPt()), horiz[0]->getOrientation()))

    NEW_SHAPE(botBorder, LineData, new LineData(camShS, bottomMost(vert[0]->bottomPt(), vert[1]->bottomPt()), horiz[1]->getOrientation()))

    NEW_SKETCH(board, bool, visops::rightHalfPlane(leftBorder) & visops::leftHalfPlane(rtBorder) & visops::bottomHalfPlane(topBorder) & visops::topHalfPlane(botBorder))

    NEW_SKETCH(leftCol, bool, board & visops::leftHalfPlane(vert[0]))
    NEW_SKETCH(rtCol, bool, board & visops::rightHalfPlane(vert[1]))
    NEW_SKETCH(midCol, bool, board & !(leftCol | rtCol))

    std::vector<Sketch<bool> > cols(3);
    cols[0] = leftCol;
    cols[1] = midCol;
    cols[2] = rtCol;

    NEW_SKETCH(topRow, bool, board & visops::topHalfPlane(horiz[0]))
    NEW_SKETCH(botRow, bool, board & visops::bottomHalfPlane(horiz[1]))
    NEW_SKETCH(midRow, bool, board & !(topRow | botRow))

    std::vector<Sketch<bool> > rows(3);
    rows[0] = topRow;
    rows[1] = midRow;
    rows[2] = botRow;

    std::vector<Sketch<bool> > cells(9);
    
    for(int r = 0; r < 3; r++)
    {
        for(int c = 0; c < 3; c++)
        {
            cells[r*3 + c] = cols[c] & rows[r];
        }
    }

    NEW_SKETCH(botPink, bool, visops::zeros(board))
    NEW_SKETCH(botBlue, bool, visops::zeros(board))

    NEW_SHAPEVEC(ell, EllipseData, select_type<EllipseData>(camShS))
    NEW_SHAPEVEC(pinkEll, EllipseData, subset(ell, IsColor("pink")))
    NEW_SHAPEVEC(blueEll, EllipseData, subset(ell, IsColor("blue")))

    SHAPEVEC_ITERATE(pinkEll, EllipseData, p)
        Sketch<bool> pS = p->getRendering();
        botPink |= (pS & ! pS[*camSkS.idxS]);
    END_ITERATE
    
    SHAPEVEC_ITERATE(blueEll, EllipseData, b)
        Sketch<bool> bS = b->getRendering();
        botBlue |= (bS & ! bS[*camSkS.idxS]);
    END_ITERATE

    NEW_SKETCH(result, uchar, visops::zeros(board))

    NEW_SKETCH(cell, bool, visops::zeros(board))

    for(int i = 0; i < 9; i++)
    {
        cell = cells[i];

        if(!(botPink & cell)->empty())
        {
            result = visops::maskedAssign(result, cell, pink_index);
        }
        else if(!(botBlue & cell)->empty())
        {
            result = visops::maskedAssign(result, cell, blue_index);
        }
    }

    printf("Parsing done\n");
        

   }            
};

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

#endif