#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q4 : VisualRoutinesStateNode {

  // Looks at scene and extracts ellipses
  $nodeclass AddToMap : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    cout << "adding to my map" << endl;
    mapreq.addObjectColor(lineDataType, "blue");
    //mapreq.pursueShapes = true;
    cout << "done adding to my map" << endl;
  }

  // Examines the scene and reports number of ellipses
  $nodeclass Report : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));

    if (lines.size() < 1) {
      cout << "Could not find enough lines to determine square. " << lines.size() << endl;
      postStateFailure();
      return;
    }
    if (lines[0]->getLength() < 500) {
      cout << "Could not see a good enough line." << endl;
      postStateFailure();
      return;
    }

    vector<Point> sqpt;

    Point A = lines[0]->end1Pt();
    Point B = lines[0]->end2Pt();

    // Do some math like a boss
    float ox = A.coordX();
    float oy = A.coordY();
    float da = ox - B.coordX();
    float db = oy - B.coordY();
    cout << "Differences: " << da << ", " << db << endl;

    Point C = Point(ox-db,oy-da);
    Point D = Point(C.coordX() + da, C.coordY() - db);

    sqpt.push_back(A);
    sqpt.push_back(B);
    sqpt.push_back(D);
    sqpt.push_back(C);

    NEW_SHAPE(square, PolygonData, 
      new PolygonData(worldShS, sqpt, true, true, true));

//    SHAPEVEC_ITERATE(lines, LineData, l)

//      if(l->getLength() > 700 && l->getLength() < 800) {
//         cout << "found a full edge" << endl;
//         edge = l->getLength();
//      }
//    END_ITERATE;

    cout << "I " << (square->isInside(theAgent->getCentroid()) ? " AM " : " AM NOT ") << " inside the square." << endl;
    postStateSuccess();
  }  

  $setupmachine {
    a : AddToMap =C=> r
    r : Report
    r =S=> SpeechNode("done")
    r =F=> Turn(M_PI/6) =C=> a

  }
    //AddToMap =C=> Turn(-M_PI/2) =C=> AddToMap =C=> Turn(-M_PI/2) =C=> AddToMap =C=> Turn(-M_PI/2) =C=
}

REGISTER_BEHAVIOR(Lab4Q4);