#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q4 : VisualRoutinesStateNode {
  $provide int turnCount;
  
  enum outcome {inSquare, outSquare, keepLooking};
  
  virtual void doStart() {
    turnCount = 0;
  }

  $nodeclass FindIt : MapBuilderNode : doStart{
    mapreq.addObjectColor(lineDataType, "blue");
  }

  $nodeclass ProjectIt : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    mapreq.clearCamera = false;  
  }
  
  $nodeclass CompleteIt : MapBuilderNode : doStart{
    $reference Lab4Q4::turnCount; 
    
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));
    
    SHAPEVEC_ITERATE(lines, LineData, line){
      line->setInfinite();
    }END_ITERATE;
    
    LineData::PerpendicularTest isPerp(0.785);
    vector < Point > square;
    
    int done = 0;
    if(lines.size() > 1) {
      for (unsigned int i = 0; !done && i < lines.size() - 1; i++) {
        for (unsigned int j = i+1; !done && j < lines.size(); j++) {
          if (isPerp(lines[i],lines[j])) {
            done = 1;
            Point corner = lines[i]->intersectionWithLine(lines[j]);
            square.push_back(corner);
            Point corner2 = corner + (lines[i]->getCentroid() - corner).unitVector() * 750.0f;
            square.push_back(corner2);
            Point corner3 = corner + (lines[j]->getCentroid() - corner).unitVector() * 750.0f;

            Point corner4 = corner + (lines[i]->getCentroid() - corner).unitVector() * 750.0f + 
                                     (lines[j]->getCentroid() - corner).unitVector() * 750.0f;
            square.push_back(corner4);
            square.push_back(corner3);
          }
        }
      }
      NEW_SHAPE(poly, PolygonData, new PolygonData(localShS, square, true));
    
      if ( poly->isInside(Point(0,0))) {
        cout << "The robot is inside the square." << endl;
        postStateSignal<outcome>(inSquare);
      } else {
        cout << "The robot is not inside the square." << endl;
        postStateSignal<outcome>(outSquare);
      }
    } else {
      if (++turnCount > 8) {
        cout << "The robot is inside the square." << endl;
        postStateSignal<outcome>(inSquare);
      } else {
        postStateSignal<outcome>(keepLooking);
      }
    } 
    
  }
  
  $setupmachine{
  
    startState2: StateNode 
    completed: CompleteIt
    startState2 =N=> FindIt =C=> ProjectIt =C=> completed
    completed =S<outcome>(keepLooking)=> SpeechNode("notdone!") =C=> Turn(0.785) =C=> startState2
    completed =S<outcome>(inSquare)=> SpeechNode("in the square") 
    completed =S<outcome>(outSquare)=> SpeechNode("outside the square") 

  }

}

REGISTER_BEHAVIOR(Lab4Q4);