#include "Shared/RobotInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"

$nodeclass Lab7p5 {
  
  //ellipse center x and y
  $provide float ellipse_x;
  $provide float ellipse_y;
  $provide float ori_angle;

  //do more tile each time
  $nodeclass HeadTilt : PostureNode : doStart {
    static float tiltAngle = 0;
    tiltAngle -= 0.1;
    getMC()->setOutputCmd( HeadTiltOffset ,tiltAngle);
  }

  //find shapes in local map
  $nodeclass FindShapes : MapBuilderNode($,MapBuilderRequest::localMap) : doStart {
    mapreq.addObjectColor(ellipseDataType,"red");
  }

  //find ellipse in local map
  enum choice {found_ellipse, no_ellipse};
  $nodeclass GetEllipseLocation : StateNode : doStart {
    $reference Lab7p5::ellipse_x;
    $reference Lab7p5::ellipse_y;
    NEW_SHAPEVEC(red_ellipses, EllipseData, select_type<EllipseData>(localShS));
    cout << "finding..." << endl;
    if (red_ellipses.size() == 1)
    {
      ellipse_x = (red_ellipses[0]->getCentroid()).coords[0];
      ellipse_y = (red_ellipses[0]->getCentroid()).coords[1];
      cout << "I get a ellipse in local map at x=" << ellipse_x << " y=" << ellipse_y << endl;
      postStateSignal<choice>(found_ellipse);
    }
    else
    {
      cout << "I found no ellipse" << endl;
      postStateSignal<choice>(no_ellipse);
    }
  }
  
  // say hello to test
  $nodeclass SayHello : StateNode : doStart {
    cout << "Hello ~~~" << endl;
  }

  $nodeclass MoveRound : PostureNode {
    $provide float maxPan;
    virtual void doStart() {
      $reference Lab7p5::ellipse_x;
      $reference Lab7p5::ellipse_y;
      $reference Lab7p5::ori_angle;
      ori_angle = atan( abs(ellipse_x) / abs(ellipse_y) );
      maxPan = 1.09956;
      erouter->addListener(this, EventBase::sensorEGID);
    }

    virtual void doEvent() {
      $reference Lab7p5::ellipse_x;
      $reference Lab7p5::ellipse_y;
      $reference Lab7p5::ori_angle;
      float cPan;
      float beta;
      float theta;
      float gamma;
      float targetx;
      float targety;
      float targetrad;
      getMC()->takeSnapshot();
      cPan = (getMC()->getOutputCmd(HeadPanOffset)).value;
      // calculate target x,y,z and orientation
      if (ellipse_y <= 0)
      {
          if (cPan<=0)
          {
            beta = ori_angle;
            theta = abs(cPan) * (M_PI/2) / maxPan;
            gamma = beta - theta;
            targetx = ellipse_x - (100*cos(gamma));
            targety = ellipse_y + (100*sin(gamma));
            targetrad = -gamma;
          }
          else
          {
            beta = ori_angle;
            theta = abs(cPan) * (M_PI/2) / maxPan;
            gamma = beta + theta;
            targetx = ellipse_x - (100*cos(gamma));
            targety = ellipse_y + (100*sin(gamma));
            targetrad = -gamma;
          }
      }
      else
      {
          if (cPan<=0)
          {
            beta = ori_angle;
            theta = abs(cPan) * (M_PI/2) / maxPan;
            gamma = beta + theta;
            targetx = ellipse_x - (100*cos(gamma));
            targety = ellipse_y - (100*sin(gamma));
            targetrad = gamma;
          }
          else
          {
            beta = ori_angle;
            theta = abs(cPan) * (M_PI/2) / maxPan;
            gamma = beta - theta;
            targetx = ellipse_x - (100*cos(gamma));
            targety = ellipse_y - (100*sin(gamma));
            targetrad = gamma;
          }
      }

      fmat::Column<3> target = fmat::pack(targetx,targety,16);
      fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(targetrad);
      fmat::Column<3> posOffset = fmat::pack(0,0,0);
      fmat::Quaternion oriOffset = fmat::Quaternion::identity();
      cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
      bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
      cout << "success=" << success << endl;
    }
  }

  $setupmachine {
    startnode: StateNode
    moveround: MoveRound
    tilthead: HeadTilt
    findstuff: FindShapes
    findellipse: GetEllipseLocation
    sayhello: SayHello
    startnode =N=> tilthead =C=> findstuff =C=> findellipse
    findellipse =S<choice>(found_ellipse)=> sayhello =N=> moveround
    findellipse =S<choice>(no_ellipse)=> startnode
  }
}

REGISTER_BEHAVIOR(Lab7p5);