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

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

using namespace DualCoding;

class BoardParserBehavior : public VisualRoutinesStateNode {
public:
  BoardParserBehavior(): VisualRoutinesStateNode("BoardParserBehavior") {}
  
class BuildBoardParser : public VisualRoutinesStateNode {
public:
  BuildBoardParser() : VisualRoutinesStateNode("BuildCamMap") {}
  
  virtual void DoStart() {
    
    //**************** Set up map builder request ****************
    const int pink_index = ProjectInterface::getColorIndex("pink");
    const int yellow_index = ProjectInterface::getColorIndex("yellow");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    MapBuilderRequest req(MapBuilderRequest::cameraMap);
	req.maxDist = 1500;
	req.numSamples = 1;
	req.clearShapes = true;

    req.objectColors[lineDataType].insert(yellow_index);
    req.occluderColors[lineDataType].insert(pink_index);
    req.occluderColors[lineDataType].insert(blue_index);

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

	req.immediateRequest = true;
	unsigned int mapreq_id = mapBuilder.executeRequest(req);
	postStateCompletion();

  }

};


	class ExamineBoard : public VisualRoutinesStateNode {
	public:
		ExamineBoard() : VisualRoutinesStateNode("ExamineMapLab") {}

		virtual void DoStart() {
    	sndman->playFile("barklow.wav");
    	cout << "MapBuilder found " << camShS.allShapes().size() << " shapes." << endl;
			Shape<LineData>topHoriz;
			Shape<LineData>bottomHoriz;
			Shape<LineData>leftVert;
			Shape<LineData>rightVert;
			Shape<LineData>topBdLine;
			Shape<LineData>bottomBdLine;
			Shape<LineData>rightBdLine;
			Shape<LineData>leftBdLine;
			NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));

			/* get indices of longest horizontal lines */
			int i = 0;
			int hmax_1 = 0;
			int hmax_2 = 1;
			NEW_SHAPEVEC(hlines, LineData, subset(lines, LineData::IsHorizontal()));
			SHAPEVEC_ITERATE(hlines, LineData, ln)
				if (i < 2) continue;
				if (LineData::LengthLessThan()(hlines[hmax_1], ln)) {
					hmax_2 = hmax_1;
					hmax_1 = i;
				}
				else if (LineData::LengthLessThan()(hlines[hmax_2], ln)) {
					hmax_2 = i;
				}
				i++;
			END_ITERATE;
			cout << "Longest horizontal hlines: idx " << hmax_1 << " id " << hlines[hmax_1]->getId() << " and idx " << hmax_2 << " id " << hlines[hmax_2]->getId() << endl;
		topHoriz = (IsAbove()(hlines[hmax_1]->topPtShape(), hlines[hmax_2]->topPtShape())) ? hlines[hmax_1] : hlines[hmax_2];
		bottomHoriz = (IsAbove()(hlines[hmax_1]->topPtShape(), hlines[hmax_2]->topPtShape())) ? hlines[hmax_2] : hlines[hmax_1];

		/* get indices of longest vertical lines */
		i = 0;
		int vmax_1 = 0;
		int vmax_2 = 1;
		NEW_SHAPEVEC(vlines, LineData, subset(lines, not1(LineData::IsHorizontal())));
		SHAPEVEC_ITERATE(vlines, LineData, ln)
			if (i < 2) continue;
			if (LineData::LengthLessThan()(vlines[vmax_1], ln)) {
				vmax_2 = vmax_1;
				vmax_1 = i;
			}
			else if (LineData::LengthLessThan()(vlines[vmax_2], ln)) {
				vmax_2 = i;
			}
			i++;
		END_ITERATE;
		cout << "Longest vertical lines: idx " << vmax_1 << " id " << vlines[vmax_1]->getId() << " and idx " << vmax_2 << " id " << vlines[vmax_2]->getId() << endl;

		leftVert = (IsLeftOf()(vlines[vmax_1]->leftPtShape(), vlines[vmax_2]->leftPtShape())) ? vlines[vmax_1] : vlines[vmax_2];
		rightVert = (IsLeftOf()(vlines[vmax_1]->leftPtShape(), vlines[vmax_2]->leftPtShape())) ? vlines[vmax_2] : vlines[vmax_1];

		/* construct outside bounding lines */
		/** left bding line **/
		if (IsLeftOf()(hlines[hmax_1]->leftPtShape(), hlines[hmax_2]->leftPtShape())) {
			leftBdLine = new LineData(camShS, hlines[hmax_1]->leftPt(), leftVert->getOrientation());
		}
		else {
			leftBdLine = new LineData(camShS, hlines[hmax_2]->leftPt(), leftVert->getOrientation());
		}

		/** right bding line **/
		if (IsLeftOf()(hlines[hmax_1]->leftPtShape(), hlines[hmax_2]->leftPtShape())) {
			rightBdLine = new LineData(camShS, hlines[hmax_2]->rightPt(), rightVert->getOrientation());
		}
		else {
			rightBdLine = new LineData(camShS, hlines[hmax_1]->rightPt(), rightVert->getOrientation());
		}

		/** top bding line **/
		if (IsAbove()(vlines[vmax_1]->topPtShape(), vlines[vmax_2]->topPtShape())) {
			topBdLine = new LineData(camShS, vlines[vmax_1]->topPt(), topHoriz->getOrientation());
		}
		else {
			topBdLine = new LineData(camShS, vlines[vmax_2]->topPt(), topHoriz->getOrientation());
		}

		/** bottom bding line **/
		if (IsAbove()(vlines[vmax_1]->bottomPtShape(), vlines[vmax_2]->bottomPtShape())) {
			bottomBdLine = new LineData(camShS, vlines[vmax_2]->bottomPt(), bottomHoriz->getOrientation());
		}
		else {
			bottomBdLine = new LineData(camShS, vlines[vmax_1]->bottomPt(), bottomHoriz->getOrientation());
		}

			topBdLine->V("topBdLine");
			bottomBdLine->V("bottomBdLine");
			rightBdLine->V("rightBdLine");
			leftBdLine->V("leftBdLine");

		/* construct a sketch for each square */
		/** start by finding half planes for each line 
		 * (will use only left and top) 
		**/
		NEW_SKETCH(leftPlane1, bool, visops::leftHalfPlane(leftBdLine));
		NEW_SKETCH(leftPlane2, bool, visops::leftHalfPlane(leftVert));
		NEW_SKETCH(leftPlane3, bool, visops::leftHalfPlane(rightVert));
		NEW_SKETCH(leftPlane4, bool, visops::leftHalfPlane(rightBdLine));

		NEW_SKETCH(topPlane1, bool, visops::topHalfPlane(topBdLine));
		NEW_SKETCH(topPlane2, bool, visops::topHalfPlane(topHoriz));
		NEW_SKETCH(topPlane3, bool, visops::topHalfPlane(bottomHoriz));
		NEW_SKETCH(topPlane4, bool, visops::topHalfPlane(bottomBdLine));

		NEW_SKETCH(tlsq, bool, !leftPlane1 & !topPlane1 & leftPlane2 & topPlane2);
		NEW_SKETCH(tmsq, bool, !leftPlane2 & !topPlane1 & leftPlane3 & topPlane2);
		NEW_SKETCH(trsq, bool, !leftPlane3 & !topPlane1 & leftPlane4 & topPlane2);
		NEW_SKETCH(mlsq, bool, !leftPlane1 & !topPlane2 & leftPlane2 & topPlane3);
		NEW_SKETCH(mmsq, bool, !leftPlane2 & !topPlane2 & leftPlane3 & topPlane3);
		NEW_SKETCH(mrsq, bool, !leftPlane3 & !topPlane2 & leftPlane4 & topPlane3);
		NEW_SKETCH(blsq, bool, !leftPlane1 & !topPlane3 & leftPlane2 & topPlane4);
		NEW_SKETCH(bmsq, bool, !leftPlane2 & !topPlane3 & leftPlane3 & topPlane4);
		NEW_SKETCH(brsq, bool, !leftPlane3 & !topPlane3 & leftPlane4 & topPlane4);

		NEW_SHAPEVEC(all_ellipses, EllipseData, select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(pinkels, EllipseData, subset(all_ellipses, IsColor("pink")));
		NEW_SHAPEVEC(blueels, EllipseData, subset(all_ellipses, IsColor("blue")));
		NEW_SKETCH(pinkbottoms, bool, visops::zeros(sketchFromSeg()));
		NEW_SKETCH(bluebottoms, bool, visops::zeros(sketchFromSeg()));

		SHAPEVEC_ITERATE(pinkels, EllipseData, pe)
			pinkbottoms((pe->getBoundingBox().xmin + pe->getBoundingBox().xmax)/2, 
						pe->getBoundingBox().ymax) = true;
		END_ITERATE
		SHAPEVEC_ITERATE(blueels, EllipseData, be)
			bluebottoms((be->getBoundingBox().xmin + be->getBoundingBox().xmax)/2, 
						be->getBoundingBox().ymax) = true;
		END_ITERATE

		NEW_SKETCH(r_tlsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_tmsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_trsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_mlsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_mmsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_mrsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_blsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_bmsq, uchar, visops::zeros(camSkS));
		NEW_SKETCH(r_brsq, uchar, visops::zeros(camSkS));

		NEW_SKETCH(result, uchar, visops::zeros(camSkS));

		NEW_SKETCH(bluesk, bool, visops::zeros(camSkS));
		bluesk->setColor(ProjectInterface::getColorIndex("blue"));
		bluesk = true;

		NEW_SKETCH(pinksk, bool, visops::zeros(camSkS));
		pinksk->setColor(ProjectInterface::getColorIndex("pink"));
		pinksk = true;

		NEW_SKETCH(b_temp, bool, tlsq & bluebottoms);
		NEW_SKETCH(p_temp, bool, tlsq & pinkbottoms);

		r_tlsq = Sketch<uchar>(tlsq);

		if (!(b_temp->empty())) {
			r_tlsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_tlsq *= 6;
			result += (Sketch<uchar>)r_tlsq;
  	}
		else if (!(p_temp->empty())) {
			r_tlsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_tlsq *= 5;
			result += (Sketch<uchar>)r_tlsq;
		}

		b_temp = Sketch<bool>(tmsq & bluebottoms);
		p_temp = Sketch<bool>(tmsq & pinkbottoms);

		r_tmsq = Sketch<uchar>(tmsq);

		if (!(b_temp->empty())) {
			r_tmsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_tmsq *= 6;
			result += (Sketch<uchar>)r_tmsq;
  	}
		else if (!(p_temp->empty())) {
			r_tmsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_tmsq *= 5;
			result += r_tmsq;
		}

		b_temp = Sketch<bool>(trsq & bluebottoms);
		p_temp = Sketch<bool>(trsq & pinkbottoms);

		r_trsq = Sketch<uchar>(trsq);

		if (!(b_temp->empty())) {
			r_trsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_trsq *= 6;
			result += r_trsq;
  	}
		else if (!(p_temp->empty())) {
			r_trsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_trsq *= 5;
			result += r_trsq;
		}

		b_temp = Sketch<bool>(mlsq & bluebottoms);
		p_temp = Sketch<bool>(mlsq & pinkbottoms);

		r_mlsq = Sketch<uchar>(mlsq);

		if (!(b_temp->empty())) {
			r_mlsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_mlsq *= 6;
			result += r_mlsq;
  	}
		else if (!(p_temp->empty())) {
			r_mlsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_mlsq *= 5;
			result += r_mlsq;
		}

		b_temp = Sketch<bool>(mmsq & bluebottoms);
		p_temp = Sketch<bool>(mmsq & pinkbottoms);

		r_mmsq = Sketch<uchar>(mmsq);

		if (!(b_temp->empty())) {
			r_mmsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_mmsq *= 6;
			result += r_mmsq;
  	}
		else if (!(p_temp->empty())) {
			r_mmsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_mmsq *= 5;
			result += r_mmsq;
		}

		b_temp = Sketch<bool>(mrsq & bluebottoms);
		p_temp = Sketch<bool>(mrsq & pinkbottoms);

		r_mrsq = Sketch<uchar>(mrsq);

		if (!(b_temp->empty())) {
			r_mrsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_mrsq *= 6;
			result += r_mrsq;
  	}
		else if (!(p_temp->empty())) {
			r_mrsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_mrsq *= 5;
			result += r_mrsq;
		}

		b_temp = Sketch<bool>(blsq & bluebottoms);
		p_temp = Sketch<bool>(blsq & pinkbottoms);

		r_blsq = Sketch<uchar>(blsq);

		if (!(b_temp->empty())) {
			r_blsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_blsq *= 6;
			result += r_blsq;
  	}
		else if (!(p_temp->empty())) {
			r_blsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_blsq *= 5;
			result += r_blsq;
		}

		b_temp = Sketch<bool>(bmsq & bluebottoms);
		p_temp = Sketch<bool>(bmsq & pinkbottoms);

		r_bmsq = Sketch<uchar>(bmsq);

		if (!(b_temp->empty())) {
			r_bmsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_bmsq *= 6;
			result += r_bmsq;
  	}
		else if (!(p_temp->empty())) {
			r_bmsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_bmsq *= 5;
			result += r_bmsq;
		}

		b_temp = Sketch<bool>(brsq & bluebottoms);
		p_temp = Sketch<bool>(brsq & pinkbottoms);

		r_brsq = Sketch<uchar>(brsq);

		if (!(b_temp->empty())) {
			r_brsq->setColor(ProjectInterface::getColorIndex("blue"));
			r_brsq *= 6;
			result += r_brsq;
  	}
		else if (!(p_temp->empty())) {
			r_brsq->setColor(ProjectInterface::getColorIndex("pink"));
			r_brsq *= 5;
			result += r_brsq;
		}

		result += topBdLine->getRendering();
		result += bottomBdLine->getRendering();
		result += rightBdLine->getRendering();
		result += leftBdLine->getRendering();
		result += topHoriz->getRendering();
		result += bottomHoriz->getRendering();
		result += leftVert->getRendering();
		result += rightVert->getRendering();

	}

	};
		virtual void setup() {
#statemachine
			startnode: BuildBoardParser() =C=> ExamineBoard()
#endstatemachine
		}
};

#endif