#include "Behaviors/StateMachine.h"

using namespace std;

$nodeclass HW5_Teacher : VisualRoutinesStateNode {
  $provide Point agentPos();
  $provide AngTwoPi agentDir();
  $provide vector<Shape<AprilTagData> > lmks();
  
  $nodeclass Emaze : VisualRoutinesStateNode : doStart {
    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "Building map..." << endl;
		
    // world bounds
		
    vector<Point> pts;
    pts.push_back(Point(   -457.2,      457.2));
    pts.push_back(Point(   1981.2,      457.2));
    pts.push_back(Point(   1981.2,    -1524.0));
    pts.push_back(Point(   1219.2,    -1524.0));
    pts.push_back(Point(   1219.2,     -355.6));
    pts.push_back(Point(   1143.0,     -355.6));
    pts.push_back(Point(   1143.0,    -1524.0));
    pts.push_back(Point(    381.0,    -1524.0));
    pts.push_back(Point(    381.0,     -355.6));
    pts.push_back(Point(    304.8,     -355.6));
    pts.push_back(Point(    304.8,    -1524.0));
    pts.push_back(Point(   -457.2,    -1524.0));
		
    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, pts, true));
		
    // add landmarks
		
    NEW_SHAPE(tag0, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2,    50.8, 177.8),
			       AprilTags::TagDetection(0)));
		
    NEW_SHAPE(tag1, AprilTagData,
	      new AprilTagData(worldShS, Point( -76.2,   456.2, 177.8),
			       AprilTags::TagDetection(1)));

    NEW_SHAPE(tag2, AprilTagData,
	      new AprilTagData(worldShS, Point( 762.0,   456.2, 177.8),
			       AprilTags::TagDetection(2)));

    NEW_SHAPE(tag3, AprilTagData,
	      new AprilTagData(worldShS, Point(1600.2,   456.2, 177.8),
			       AprilTags::TagDetection(3)));

    NEW_SHAPE(tag4, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2,    50.8, 177.8),
			       AprilTags::TagDetection(4)));
		
    NEW_SHAPE(tag5, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2,  -508.0, 177.8),
			       AprilTags::TagDetection(5)));
		
    NEW_SHAPE(tag6, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2, -1371.6, 177.8),
			       AprilTags::TagDetection(6)));
		
    NEW_SHAPE(tag7, AprilTagData,
	      new AprilTagData(worldShS, Point(1600.2, -1523.0, 177.8),
			       AprilTags::TagDetection(7)));

    NEW_SHAPE(tag8, AprilTagData,
	      new AprilTagData(worldShS, Point(1220.2, -1371.6, 177.8),
			       AprilTags::TagDetection(8)));

    NEW_SHAPE(tag9, AprilTagData,
	      new AprilTagData(worldShS, Point(1220.2,  -508.0, 177.8),
			       AprilTags::TagDetection(9)));

    NEW_SHAPE(tag10, AprilTagData,
	      new AprilTagData(worldShS, Point(1142.0,  -508.0, 177.8),
			       AprilTags::TagDetection(10)));

    NEW_SHAPE(tag11, AprilTagData,
	      new AprilTagData(worldShS, Point(1142.0, -1371.6, 177.8),
			       AprilTags::TagDetection(11)));

    NEW_SHAPE(tag12, AprilTagData,
	      new AprilTagData(worldShS, Point( 762.0, -1523.0, 177.8),
			       AprilTags::TagDetection(12)));

    NEW_SHAPE(tag13, AprilTagData,
	      new AprilTagData(worldShS, Point( 382.0, -1371.6, 177.8),
			       AprilTags::TagDetection(13)));

    NEW_SHAPE(tag14, AprilTagData,
	      new AprilTagData(worldShS, Point( 382.0,  -508.0, 177.8),
			       AprilTags::TagDetection(14)));

    NEW_SHAPE(tag15, AprilTagData,
	      new AprilTagData(worldShS, Point( 303.8,  -508.0, 177.8),
			       AprilTags::TagDetection(15)));

    NEW_SHAPE(tag16, AprilTagData,
	      new AprilTagData(worldShS, Point( 303.8, -1371.6, 177.8),
			       AprilTags::TagDetection(16)));

    NEW_SHAPE(tag17, AprilTagData,
	      new AprilTagData(worldShS, Point( -76.2, -1523.0, 177.8),
			       AprilTags::TagDetection(17)));

    NEW_SHAPE(tag18, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2, -1371.6, 177.8),
			       AprilTags::TagDetection(18)));

    NEW_SHAPE(tag19, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2,  -508.0, 177.8),
			       AprilTags::TagDetection(19)));

    pilot->setWorldBounds(worldBounds);
  };
	
  $nodeclass StartTeacher : StateNode : doStart {
    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "Welcome to Particle Filter Bingo: Teacher Mode" << endl;
    cout << endl;
    cout << "This is a tool for teaching students about particle filters. You can randomly generate or supply the initial coordinates of the robot, and then the program finds visible landmarks. Tell the students the landmarks one at a time and they will enter the landmarks into their student mode behavior. The student mode behavior will then use a particle filter to tell the students what the best particle is. The students can then report their best particle and the student with the closest particle wins." << endl;
  }

  $nodeclass RequestAgentPosition : StateNode : doStart {
    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "Enter x, y, and theta (in degrees) for the robot, separated by spaces." << endl;
    cout << "Or enter \"random\" to randomly generate the initial position." << endl;
  }

  $nodeclass ParseAgentPosition : StateNode : doStart {
    $reference HW5_Teacher::agentPos, HW5_Teacher::agentDir;

    const TextMsgEvent *txtev = dynamic_cast<const TextMsgEvent*>(event);
    if ( txtev == NULL ) {
      cout << "Error: Null text event" << endl;
      postStateFailure(); // Failure retries
      return;
    }

    cout << "Parsing agent position..." << endl;
    std::istringstream is(txtev->getText());

    // Read in the XY position and the angle as degrees
    float x, y, th;
    is >> x >> y >> th;
    agentPos.setCoords(x, y);
    agentDir = AngTwoPi(th * Pi / 180);

    GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
    if (worldBounds->isInside(agentPos)) {
      postStateSuccess(); // Success continues
    } else {
      cout << "Error: Coordinates not in the world" << endl;
      postStateFailure();
    }
  }

  $nodeclass GenerateAgentPosition : StateNode : doStart {
    $reference HW5_Teacher::agentPos, HW5_Teacher::agentDir;
    
    cout << endl;
    cout << "Generating random agent position..." << endl;

    // particle index 0 is the zero position so use index 1 instead
    particleFilter->resetFilter();
    LocalizationParticle& agentParticle = particleFilter->getParticles()[1];
    agentPos.setCoords(agentParticle.x, agentParticle.y);
    agentDir = agentParticle.theta;
  }

  $nodeclass ReportAgentPosition : StateNode : doStart {
    $reference HW5_Teacher::agentPos, HW5_Teacher::agentDir;
    
    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "Using robot position:" << endl;
    cout << "X: " << agentPos.coordX() << endl;
    cout << "Y: " << agentPos.coordY() << endl;
    cout << "TH: " << agentDir << endl;

    cout << endl;

    // Use a union of floats and ints to get the binary representation of the position and heading, then output this in hex

    union {
      struct {
        float x, y, th;
      } fval;
      struct {
        int a, b, c;
      } ival;
    } hexConverter;

    hexConverter.fval.x = agentPos.coordX();
    hexConverter.fval.y = agentPos.coordY();
    hexConverter.fval.th = agentDir;

    cout << "The hexadecimal encoded string is: ";
    cout << hex << hexConverter.ival.a;
    cout << hexConverter.ival.b;
    cout << hexConverter.ival.c;
    cout << dec << endl;

    pilot->setAgent(agentPos, agentDir, true, false);
  }

  static bool isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
                                 const DualCoding::Point &currentLocation,
                                 AngTwoPi currentOrientation, AngPi maxTurn) {
    AngSignPi bearing = (selectedLandmark->getCentroid() - currentLocation).atanYX();

    // is the camera within our possible field of view (in front of us)?
    // NOTE: The original code from Dave had an error in the following line (currentOrientation was never used)
    if ( fabs(bearing-currentOrientation) > (CameraHorizFOV/2.0+maxTurn) )
      return false;

    GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
    // If there are no walls, assume all landmarks are visible,
    // but this isn't really true for poster-type markers where
    // we need to know the viewing angle.  Will fix this someday.
    if ( ! worldBounds.isValid() )
      return true;
    // do we intersect with the world bounds when looking at marker?
    LineData lineOfSight(VRmixin::worldShS, currentLocation, selectedLandmark->getCentroid());
    if (worldBounds.isValid() && worldBounds->intersectsLine(lineOfSight))
      return false;

    // this landmark is viewable
    return true;
  }

  $nodeclass GetVisibleLandmarks(unsigned int maxLmks) : StateNode : doStart {
    $reference HW5_Teacher::agentPos, HW5_Teacher::agentDir, HW5_Teacher::lmks;
    
    // get all the tags and iterate through them to find the visible ones
    NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));

    // Put up to maxLmks landmarks into the list
    SHAPEVEC_ITERATE(tags, AprilTagData, t) {
      if (isLandmarkViewable(t, agentPos, agentDir, Pi/4) && lmks.size()<maxLmks) {
        lmks.push_back(t);
      }
    } END_ITERATE;
  }

  $nodeclass BeginReportLandmarks : StateNode : doStart {
    $reference HW5_Teacher::lmks;

    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "Now you need to tell your students some landmarks." << endl;
    cout << "First tell your students there are " << lmks.size() << " landmarks." << endl;
  
    cout << endl;
    cout << "Now this will display one landmark at a time. Enter \"next\" to view the next landmark." << endl;
  }

  $nodeclass ReportLandmark : StateNode : doStart {
    $reference HW5_Teacher::agentPos, HW5_Teacher::agentDir, HW5_Teacher::lmks;
    
    cout << endl;

    if (lmks.empty()) {
      // No landmarks left
      cout << "Error: No landmarks" << endl;
      postStateFailure();
      return;
    }

    // Get the last landmark
    Shape<AprilTagData> &curLmk = lmks.back();

    // Compute the distance and angle of the landmark with respect to the agent's position and direction
    Point agent2lmk = curLmk->getCentroid() - agentPos;
    float dist = agent2lmk.xyNorm();
    float angle = agent2lmk.atanYX() - agentDir;
    
    cout << "Landmark " << curLmk.getData().getTagID() << endl;
    cout << "Distance: " << dist << endl;
    cout << "Angle: " << angle << endl;

    // Delete the last landmark from the list
    lmks.pop_back();

    if (lmks.empty()) {
      // No landmarks left
      postStateFailure();
    } else {
      // Wait for the "next" command
      postStateSuccess();
    }
  }

  $nodeclass Done : StateNode : doStart {
    cout << endl;
    cout << "----------" << endl;
    cout << endl;

    cout << "There are no more landmarks." << endl;

    cout << endl;

    cout << "Now your students will tell you their best particle and error. Find the student with the lowest error and announce their results to the class." << endl;

    stop();
  }

  $setupmachine {
    startnode: StartTeacher =N=> Emaze =N=> reqAP

    // Get a valid agent position
    reqAP: RequestAgentPosition =TM=> parseAP =S=> reportAP
    parseAP: ParseAgentPosition =F=> reqAP
    
    // Or user can generate a random one
    reqAP =TM("random")=> GenerateAgentPosition =N=> reportAP
    
    reportAP: ReportAgentPosition =N=> getLmks

    getLmks: GetVisibleLandmarks($,4) =N=> BeginReportLandmarks =N=> reportLmk

    // Report landmarks one at a time until there are no more
    reportLmk: ReportLandmark =S=> waitNext
    waitNext: StateNode =TM("next")=> reportLmk
    waitNext =TM=> waitNext
    
    reportLmk =F=> Done
  }
}

REGISTER_BEHAVIOR(HW5_Teacher);