#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/VeeTags.h"
#include "Crew/PilotRequest.h"
#include "Vision/AprilTags/MathUtil.h"
#include <math.h>

// there is occasinally a bug where the robot orients the cylinder in the right
// direction, however, it is then somehow unable to push the cylinder. Since
// the move-forward command seems to be completing and since
// the cylinders are light enough for this to be unlikely physically, could
// there be something wrong with the simulation?

$nodeclass CylinderPusher : PilotDemo {

  // sets robot heading
  $nodeclass AdjustHeading : PilotNode(PilotTypes::walk) : doStart {
    Point loc = theAgent->getCentroid();
    float currHead = float(theAgent->getOrientation());
    float desiredHead = 3.1416 + atan2(float(loc.coordY()),float(loc.coordX()));
    float turnHead = AprilTags::MathUtil::mod2pi(desiredHead-currHead);
    cout << "Robot turning " << turnHead << endl;
    pilotreq.da = turnHead;
    pilotreq.collisionAction = collisionIgnore;
  }

  // this node assumes that robot heading is correct
  $nodeclass Advance : PilotNode(PilotTypes::walk) : doStart {
    Point loc = theAgent->getCentroid();
    float distance = min(loc.xyzNorm(),float(1000.0)); 
    cout << "Robot advancing " << distance << endl;
    pilotreq.collisionAction = collisionIgnore;
  }

  $nodeclass CheckLocation : StateNode : doStart {
    Point loc = theAgent->getCentroid();
    float distance = loc.xyzNorm(); 
    
    if (distance < 500.0) {
      postStateSuccess();
    } else {
      postStateFailure();
    }
  }

  $setupmachine {
    
    rundemo: AdjustHeading // we start off with a pose check

    orient: AdjustHeading
    move: Advance
    check: CheckLocation

    // for some reason, it does not appear possible to run this transition
    // more than twice in the same run of the tekkotsu-CREATE script
    rundemo =C=> check

    // movement is done in steps in case the weight of the cylinder messes up
    // the robot's trajectory
    check =F=> orient
    check =S=> SoundNode("howl.wav")

    orient =C=> move
    move =C=> check //move trajectory updates


  }
}

REGISTER_BEHAVIOR(CylinderPusher);