//-*-c++-*-
#ifndef INCLUDED_Lab8d_h_
#define INCLUDED_Lab8d_h_

#include "DualCoding/DualCoding.h" 
#include "Behaviors/StateNode.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Events/EventRouter.h"
#include "Sound/SoundManager.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Motion/Kinematics.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/SoundNode.h"
#include "Events/VisionObjectEvent.h"
#include "Motion/HeadPointerMC.h"
#include "Shared/ERS7Info.h"
#include "Motion/MMAccessor.h"
#include "Motion/PostureMC.h"
#include "DualCoding/VisualRoutinesStateNode.h"

using namespace DualCoding;

class PointAtBlobNode : public VisualRoutinesStateNode {
protected:  
  SharedObject<PostureMC> pos_mc;
  MotionManager::MC_ID pos_id;
  const int orange_index;
  
public:
  PointAtBlobNode(std::string nodename = "PointAtBlobNode"): VisualRoutinesStateNode("PointAtBlobNode",nodename), pos_mc(), pos_id(MotionManager::invalid_MC_ID), orange_index(ProjectInterface::getColorIndex("orange")) {}
  
  virtual void DoStart() {
    VisualRoutinesStateNode::DoStart();
    std::cout << getName() << " is starting up." << std::endl;
    
    localShS.clear(); // clear manually so we don't lose gazePoly
    
    vector<Point> gazePts;
    gazePts.push_back(Point(220,80,-50));
    gazePts.push_back(Point(450,300,-50));
    gazePts.push_back(Point(800,0,-50));
    gazePts.push_back(Point(450,-300,-50));
    gazePts.push_back(Point(220,-80,-50));
    gazePts.push_back(Point(500,0,-50));
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, false));
    
    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePoly;
    req.pursueShapes = true;
    req.maxDist = 1000;
    req.rawY = true;
    req.clearShapes = false;
    req.objectColors[blobDataType].insert(orange_index);
    
    unsigned int mapreq_id = mapBuilder.executeRequest(req);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::deactivateETID);
  }
  
  
  virtual void processEvent(const EventBase &event) {
    if(event.getGeneratorID() == EventBase::mapbuilderEGID){
      cout << "MapBuilder returned " << event.getDescription() << endl;
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      if(blobs.size() == 0){
	cout << "No orange objects" << endl;
	sndman->playFile("barklow.wav");
	postCompletionEvent();
      }
      else{
	NEW_SHAPE(biggestblob, BlobData, max_element(blobs,BlobData::AreaLessThan()));    
	Point blobLoc = biggestblob->getCentroid();
	
	cout << "Orange Blob Loc " << blobLoc << endl;
	NEWMAT::ColumnVector Pleftpaw = kine->getLinkInterestPoint(KneeOffset + LFrLegOffset, "ToeLFrPaw");
	cout << "PLeftPae " << Pleftpaw <<endl;
	if(pos_mc->solveLinkVector(Kinematics::pack(blobLoc.coordX(), blobLoc.coordY(), blobLoc.coordZ()), KneeOffset + LFrLegOffset, Kinematics::pack(Pleftpaw(1), Pleftpaw(2), Pleftpaw(3)))){
	  pos_id = motman->addPersistentMotion(pos_mc);   
	  erouter->addListener(this, EventBase::motmanEGID, pos_id, EventBase::statusETID);
	}
	else{
	  cout << "Couldn't solve kinematics" << endl;
	  sndman->playFile("barklow.wav");
	  postCompletionEvent();
	}
      }
    }
    else if(event.getGeneratorID() == EventBase::motmanEGID){
      postCompletionEvent();
    }

  }
  
  virtual void DoStop() {
    std::cout << getName() <<" is shutting down." << std::endl; 
    motman->removeMotion(pos_id);
    pos_id = MotionManager::invalid_MC_ID;
    erouter->removeListener(this);
    StateNode::DoStop();
    }
    
};

class PawNeutralNode : public StateNode {
protected:  
  SharedObject<PostureMC> pos_mc;
  MotionManager::MC_ID pos_id;
  
public:
  PawNeutralNode(std::string nodename = "PawNeutralNode"): StateNode("PawNeutralNode",nodename), pos_mc(), pos_id(MotionManager::invalid_MC_ID) {}
  
  virtual void DoStart() {
    StateNode::DoStart();
    std::cout << getName() << " is starting up." << std::endl;
    
    NEWMAT::ColumnVector Pleftpaw = kine->getLinkInterestPoint(PawFrameOffset + LFrLegOrder, "ToeLFrPaw");
    if(pos_mc->solveLinkPosition(Kinematics::pack(122.706, 70.8322, -132.406), PawFrameOffset + LFrLegOrder, Kinematics::pack(Pleftpaw(1), Pleftpaw(2), Pleftpaw(3)))){
      pos_id = motman->addPersistentMotion(pos_mc);   
      erouter->addListener(this, EventBase::motmanEGID, pos_id, EventBase::statusETID);
    }
    else{
      cout << "Couldn't solve kinematics" << endl;
      sndman->playFile("barklow.wav");
      postCompletionEvent();
    }
    
  }
  
  virtual void DoStop() {
    std::cout << getName() <<" is shutting down." << std::endl; 
    motman->removeMotion(pos_id);
    pos_id = MotionManager::invalid_MC_ID;
    erouter->removeListener(this);
    StateNode::DoStop();
  }
  
  virtual void processEvent(const EventBase& event) {
    postCompletionEvent();
  }    
    
};



class Lab8d : public StateNode {
private:
  SharedObject<PostureMC> pos_mc;
  MotionManager::MC_ID pos_id;
  
public:
  Lab8d() : StateNode("Lab8c"), pos_mc("situp.pos"), pos_id(MotionManager::invalid_MC_ID) {}
  
  virtual void setup() {
    StateNode::setup();

#statemachine
    
  launch:StateNode == n:NullTrans ==> look
      
      look: HeadPointerNode[getMC()->lookAtPoint(100,75,-200);]
      == CompletionTrans ==> 
      wait5: StateNode

      wait5 == TimeOutTrans($,5000) ==>
      pointAtBlob: PointAtBlobNode

      pointAtBlob == CompletionTrans ==> wait1: StateNode
      
      wait1 == TimeOutTrans($,1000) ==> moveBack: PawNeutralNode

      moveBack == CompletionTrans ==> look
      
      #endstatemachine
    
      startnode = launch;
  }       // end of setup()

  void DoStart() {
    StateNode::DoStart();
    pos_id = motman->addPersistentMotion(pos_mc);
  }
   
  void DoStop(){
    motman->removeMotion(pos_id);
    pos_id = MotionManager::invalid_MC_ID;
    StateNode::DoStop();
  }

   
};


  

#endif