#ifndef _GraspDemo_h_
#define _GraspDemo_h_

#include "Behaviors/Nodes/MotionSequenceNode.h"
#include "Behaviors/Nodes/WalkNode.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Events/DataEvent.h"
#include "Events/LocomotionEvent.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/WalkMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/WorldState.h"

#include "DualCoding/DualCoding.h"
using namespace DualCoding;

enum TargetAction { approach, acquire };

std::istream& operator>>(std::istream &s, TargetAction &);

//**************** TrackBone ****************

//! Keep the camera pointed at the bone
class TrackBone : public VisualRoutinesStateNode {
 public:
  TrackBone() : VisualRoutinesStateNode("TrackBone"),
    head_mc(), head_id(MotionManager::invalid_MC_ID) {}

  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "TrackBone starting up." << endl;
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addTimer(this,1,500,true);
    }

  void DoStop() {
    motman->removeMotion(head_id);
    cout << "TrackBone stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }

  void processEvent(const EventBase &) {
    camShS.clear();
    const color_index pink_index = ProjectInterface::getColorIndex("pink");
    NEW_SHAPEVEC(pinkblobs, BlobData, 
		 getBlobsFromRegionGenerator(pink_index,30,BlobData::groundplane,1));
    projectToGround();
    if ( groundShS.allShapes().size() == 0 ) {
      // cout << "TrackBone found no pink blob" << endl;
      return;
    }
    Shape<BlobData> groundbone = ShapeRootType(groundShS.allShapes()[0],BlobData);
    Point target = groundbone->getCentroid();
    // cout << "TrackBone target = " << target << endl;
    MMAccessor<HeadPointerMC>(head_id)->lookAtPoint(target.coordX(),target.coordY(),target.coordZ());
  }

 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
};

#define CALC_ORIENT \
  GET_SHAPE(pt1,PointData,localShS); \
  GET_SHAPE(pt2,PointData,localShS); \
  Point const midpt = (pt1->getCentroid() + pt2->getCentroid())/2; \
  AngSignPi const midbearing = atan2(midpt.coordY(),midpt.coordX()); \
  Point const diff = (pt2->getCentroid() - pt1->getCentroid()); \
  AngPi const local_orient = atan2(diff.coordY(),diff.coordX()); \
  AngSignPi const perp_heading = float(local_orient) - AngPi(M_PI/2); \
  AngSignPi const adist = midbearing + perp_heading; \
  float const dist = 150; \

/*
  cout << "midbearing = " << float(midbearing)*180/M_PI						\
       << "    local_orient = " << float(local_orient)*180/M_PI		\
       << "    perp_heading = " << float(perp_heading)*180/M_PI \
       << "    adist = " << float(adist)*180/M_PI << endl;
*/

//**************** CalcDest ****************
//! Calculate destination point for approach
class CalcDest : public VisualRoutinesStateNode {
 public:
  CalcDest() : VisualRoutinesStateNode("CalcDest") {}

  void DoStart() {
    VisualRoutinesStateNode::DoStart();

    camSkS.clear();
    camShS.clear();
    const color_index pink_index = ProjectInterface::getColorIndex("pink");
    NEW_SHAPEVEC(pinkblobs, BlobData, 
		 getBlobsFromRegionGenerator(pink_index,30,BlobData::groundplane,1));
    if ( pinkblobs.size() == 0 ) {
      cout << "CalcDest: no pink blobs" << endl;
      DoStop();
      return;
    }
    Shape<BlobData> cambone = pinkblobs[0];;
    NEW_SKETCH(pinkrend,bool,cambone->getRendering());
    Region bone_reg(camSkS);
    bone_reg.addIndices(pinkrend);
    const AngPi cam_orient = bone_reg.findPrincipalAxisOrientation();
    cout << "CalcDest: bone orientation in cam space = " << float(cam_orient)*180/M_PI << " deg" << endl;
    if ( cam_orient > M_PI/2 ) {
      NEW_SHAPE(pt1, PointData, new PointData(camShS, cambone->bottomLeft));
      NEW_SHAPE(pt2, PointData, new PointData(camShS, cambone->topRight));
    } else {
      NEW_SHAPE(pt1, PointData, new PointData(camShS, cambone->topLeft));
      NEW_SHAPE(pt2, PointData, new PointData(camShS, cambone->bottomRight));
    }

    groundShS.clear();
    projectToGround();
    localShS.clear();
    localShS.importShapes(groundShS);
    CALC_ORIENT
    float const dx = dist * cos(perp_heading);
    float const dy = dist * sin(perp_heading);
    Point destpoint = midpt - Point(dx,dy,0);
    NEW_SHAPE(dest,PointData,new PointData(localShS, destpoint));
    const TargetAction a = 
      ( fabs(destpoint.coordX()-dist) > 30 || fabs(destpoint.coordY()) > 60 || fabs(adist) > 0.75 ) ?
      approach : acquire;
    erouter->postEvent(DataEvent<TargetAction>(a,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
  }

};


//**************** ApproachTarget ****************
class ApproachTarget : public WalkEngineNode<WalkMC>, public VRmixin {
 public:
  ApproachTarget() : WalkEngineNode<WalkMC>("ApproachTarget",0,0,0) {}

  void DoStart() {
    WalkEngineNode<WalkMC>::DoStart();

    GET_SHAPE(dest,PointData,localShS);
    if ( ! dest.isValid() ) {
      cout << "ApproachTarget couldn't find 'dest' in localShS" << endl;
      postCompletionEvent();
      return;
    }
    Point const destpoint = dest->getCentroid();
    CALC_ORIENT

    float dx=0, dy=0, da=0;
    if ( destpoint.coordX() > dist )
      dx = min(50.f,destpoint.coordX()-dist);
    else if ( destpoint.coordX() < dist-50.f )
      dx = max(-50.f,dist-destpoint.coordX());
    if ( destpoint.coordX() < 400 )
      if ( destpoint.coordY() > 30.f)
	dy = min(destpoint.coordY()/2,30.f);
      else if ( destpoint.coordY() < -5)
	dy = max(destpoint.coordY()/2,-30.f);
    if ( fabs(destpoint.coordX()-dist) < 100 )
      da = max(-0.2f, min(0.2f, float(adist)/4));
    cout << "Approach:  dx = " << dx << "  dy = " << dy << "  da = " << float(da)*180/M_PI << endl;
    getMC()->setSlowMo(0.5);
    getMC()->setTargetDisplacement(dx,dy,da,2);
  }

};


//**************** FindCone ****************
class FindCone : public VisualRoutinesStateNode {
 public:
  FindCone() : VisualRoutinesStateNode("FindCone"),
    head_mc(), head_id(MotionManager::invalid_MC_ID), gaze_index(0) {}

  void DoStart() {
    cout << "FindCone starting." << endl;
    VisualRoutinesStateNode::DoStart();
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID);
    moveToNextGazePoint();
  }

  void DoStop() {
    motman->removeMotion(head_id);
    VisualRoutinesStateNode::DoStop();
  }

  void processEvent(const EventBase &event) {
    if ( event.getGeneratorID() == EventBase::motmanEGID ) {
      erouter->addTimer(this,1,1000);
      return;
    }
    // event was a timer expiration: ready to take a picture now
    localShS.clear();
    int const orange_index = ProjectInterface::getColorIndex("orange");
    NEW_SHAPEVEC(oblobs,BlobData,getBlobsFromRegionGenerator(orange_index,200,BlobData::groundplane,1));
    if ( oblobs.size() == 0 )
      moveToNextGazePoint();
    else
      postCompletionEvent();
  }

  void moveToNextGazePoint() {
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,gaze_points[gaze_index],0);
    // cout << "FindCone gazing at " << gaze_points[gaze_index]*180/M_PI << endl;
    if ( gaze_points[++gaze_index] == 999 )
      gaze_index = 0;
  }

 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  int gaze_index;
  static float const gaze_points[];
};

//**************** PushToCone ****************
class PushToCone  : public VisualRoutinesStateNode {
 public:
  PushToCone() : VisualRoutinesStateNode("PushToCone"),
		 head_mc(), head_id(MotionManager::invalid_MC_ID),
		 walk_mc(), walk_id(MotionManager::invalid_MC_ID) {}

  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    head_mc->setJoints(0,state->outputs[HeadOffset+PanOffset],0);
    head_id = motman->addPersistentMotion(head_mc);
    walk_mc->LoadFile("bone/grasp.prm");
    walk_mc->setSlowMo(0.5);
    walk_id = motman->addPersistentMotion(walk_mc);
    erouter->addTimer(this,1,500,true);
  }

  void DoStop() {
    motman->removeMotion(walk_id);
    motman->removeMotion(head_id);
    VisualRoutinesStateNode::DoStop();
}

  void processEvent(const EventBase&) {
    camShS.clear();
    int const orange_index = ProjectInterface::getColorIndex("orange");
    NEW_SHAPEVEC(oblobs,BlobData,getBlobsFromRegionGenerator(orange_index,200,BlobData::groundplane,1));
    if ( oblobs.size() == 0 )
      return;
    Point opoint = oblobs[0]->getCentroid();
    // cout << "Orange blob at " << opoint <<endl;
    double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
    double const head_move = (camSkS.getWidth()/2 - opoint.coordX()) / camSkS.getWidth() * CameraFOV;
    // cout << "head_pan_angle = " << head_pan_angle*180/M_PI 
    // << "   head_move = " << head_move*180/M_PI << endl;
    double new_pan = head_pan_angle + head_move;
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,new_pan,0);
    MMAccessor<WalkMC>(walk_id)->setTargetVelocity(80,0,new_pan/2);
}

 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  SharedObject<WalkMC> walk_mc;
  MotionManager::MC_ID walk_id;
};


//**************** GraspDemo ****************
class GraspDemo : public VisualRoutinesStateNode {
public:
  GraspDemo() : VisualRoutinesStateNode("GraspDemo") {}

  void setup() {

#statemachine
  start: StateNode == multistart:NullTrans ==>
    TrackBone() == ftrans:CompletionTrans($,1) ==> 
      FindCone() == CompletionTrans ==>
        PushToCone()

  multistart ==>
    calcdest: CalcDest() == SignalTrans<TargetAction>($,approach) ==>
      ApproachTarget() == CompletionTrans ==>
        StateNode == TimeOutTrans($,500) ==> calcdest

  calcdest == SignalTrans<TargetAction>($,acquire) ==>
    TinyMotionSequenceNode("$","bone/acquire.mot") == CompletionTrans ==>
      WalkEngineNode<WalkMC>()
        [getMC()->LoadFile("bone/acquire.prm");
	      getMC()->setSlowMo(0.5);
	      setDisplacement(175, 0, 0, 5); ] == CompletionTrans ==>
        TinyMotionSequenceNode("$","bone/grasp.mot") == ftrans
#endstatemachine

    startnode = start;
  }

  void DoStart() {
    cout << "GraspDemo starting up" << endl;
    VisualRoutinesStateNode::DoStart();
    erouter->addListener(this,EventBase::textmsgEGID);
  }

  GraspDemo(const GraspDemo&); //!< do not call
  GraspDemo& operator=(const GraspDemo&); //!< do not call

};

#endif