// part 2 mapbuilder egg finder

#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"
#include "DualCoding/VisualRoutinesStateNode.h"
#include "Crew/PilotRequest.h"

$nodeclass PartFour : VisualRoutinesStateNode {

  // turns the robot around
  $nodeclass Turn : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.da = M_PI * 1/4;
    pilotreq.collisionAction = collisionIgnore;

    cout << "\nInsufficient Lines Found; Rotating Robot\n" << endl;
  }
  
  // finds lines, hopefully more than one
  $nodeclass Find : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    mapreq.addObjectColor(lineDataType, "blue");

    NEW_SHAPEVEC(lines,LineData,select_type<LineData>(camShS));
    if (lines.size() > 1) {
      postStateSuccess();
    } else {
      postStateFailure();
    }
  }

  // the exact algorithm used here is a very crude version of projecting to body coordinates and
  // then attempting to draw a quadrilateral. 
  
  // This method obviously fails if the robot cannot see any blue lines from rotation. In addition,
  // it suffers when the lines are close to the robot or when the algorithm accidentally tries to
  // compare two parallel lines - both cases may lead to serious malformation of the quadrilateral.

  // This algorithm performs best when there is some distance between the robot and the lines being
  // analyzed
  $nodeclass Evaluate : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(lines,LineData,select_type<LineData>(groundShS));

    // for convienience
    lines[0]->setInfinite(true);
    lines[1]->setInfinite(true);

    Point cornerA = lines[0]->intersectionWithLine(lines[1]);
    
    cout << "Corner found at: " << cornerA << endl;

    // along the line0 axis
    Point a_axiscen = lines[0]->getCentroid();
    float a_axisfact = 750 / cornerA.distanceFrom(a_axiscen);
    float a_xshift = a_axisfact * (a_axiscen.coordX()-cornerA.coordX());
    float a_yshift = a_axisfact * (a_axiscen.coordY()-cornerA.coordY());

    // along the line1 axis
    Point b_axiscen = lines[1]->getCentroid();
    float b_axisfact = 750 / cornerA.distanceFrom(b_axiscen);
    float b_xshift = b_axisfact * (b_axiscen.coordX()-cornerA.coordX());
    float b_yshift = b_axisfact * (b_axiscen.coordY()-cornerA.coordY());

    // generate the other three points
    Point cornerB =  Point(cornerA.coordX()+a_xshift,cornerA.coordY()+a_yshift,
				0,cornerA.getRefFrameType());
    Point cornerC =  Point(cornerA.coordX()+b_xshift,cornerA.coordY()+b_yshift,
				0,cornerA.getRefFrameType());
    Point cornerD =  Point(cornerB.coordX()+b_xshift,cornerB.coordY()+b_yshift,
				0,cornerA.getRefFrameType());

    vector<Point> pts;
    pts.push_back(cornerA);
    pts.push_back(cornerB);
    pts.push_back(cornerD);
    pts.push_back(cornerC);

    NEW_SHAPE(myPoly, PolygonData, new PolygonData(localShS, pts, true, true, true));
    myPoly->setColor("red");

    NEW_SHAPE(myEllipse,EllipseData, new EllipseData(localShS,theAgent->getCentroid(),50,50,0));
    myEllipse->setColor("green");

    bool inPoly = myPoly->isInside(theAgent->getCentroid());

    // add a return state because we can
    if (inPoly) {
      cout << "Robot is in square!" << endl; 
      postStateSuccess();
    } else {
      cout << "Robot is not in square!" << endl; 
      postStateFailure();
    }
  }

  // general behavior
  $setupmachine {
    
    rundemo : StateNode
    
    findlines : Find

    rundemo =N=> findlines

    findlines =S=> Evaluate
    findlines =F=> Turn =T(5000)=> findlines

  }
}

REGISTER_BEHAVIOR(PartFour);