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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"

using namespace DualCoding;

class Lab8 : public VisualRoutinesStateNode {
public:

	class LookLeft : public HeadPointerNode {
	public:
		LookLeft() : HeadPointerNode("LookLeft") {}

		virtual void DoStart() {
			getMC()->setJoints(0, -0.3813,-1.0723);
		}
	};

  class ArmPos : public ArmNode {
  public:
    ArmPos() : ArmNode("ArmPos") {}

    virtual void DoStart() {
      getMC()->setJoints(1.4408,-1.3691,-1.1951);
    }
  };

class Find : public VisualRoutinesStateNode {
	public:

		Find() : VisualRoutinesStateNode("ChuFindObject"), mapreq(MapBuilderRequest::localMap),mapreq_id(0){
}

		virtual void DoStart() {
			const color_index cColor_index1 = ProjectInterface::getColorIndex("green");
			const color_index cColor_index2 = ProjectInterface::getColorIndex("blue");
			const color_index line_index1 = ProjectInterface::getColorIndex("yellow");
			const color_index line_index2 = ProjectInterface::getColorIndex("pink");

			localShS.clear();
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;
			mapreq.rawY = true;
      mapreq.objectColors[lineDataType].insert(line_index1);
      mapreq.objectColors[lineDataType].insert(line_index2);
			mapreq.objectColors[ellipseDataType].insert(cColor_index1);
			mapreq.objectColors[ellipseDataType].insert(cColor_index2);

			mapreq_id = mapBuilder.executeRequest(mapreq);

      erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id);

		}
 void processEvent (const EventBase &e) {
    if ( e.getGeneratorID() == EventBase::mapbuilderEGID &&
         e.getSourceID() == mapreq_id ) {
     
//      NEW_SHAPEVEC(lines,LineData,localShS.allShapes(lineDataType));
  //    vector<ShapeRoot > ellipses = localShS.allShapes(ellipseDataType);
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(localShS))

      fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
      Point* gripperPos = new Point(fmat::SubVector<3>(&Tgripper(0,3)));
      //target[0],target[1],target[2]);    
      if (ellipses.size()==0 || ellipses.at(0)->getCentroid().xyDistanceFrom(*gripperPos)>25) {
          postStateSignal<int>(0); //error
          if(ellipses.size()!=0) {std::cout << "****** Ellipse present but too far away "<< ellipses.at(0)->getCentroid().xyDistanceFrom(*gripperPos) <<  std::endl;}
      } else {
          
  
        postStateSignal<int>(1); ///green
        //postStateSignal<int>(2); ///error
      }
    }
 }

		virtual void DoStop() {
		}

  static bool checkForBlob() {
    return find_if<BlobData>(localShS).isValid();
  }

	private:
		MapBuilderRequest mapreq;
unsigned int mapreq_id;
};

  Lab8() : VisualRoutinesStateNode("Lab8") {}

	virtual void setup() {
#statemachine
  startnode: LookLeft() =C=>
  armnode : ArmPos() =C=>
 		wait : SpeechNode("Ready") =B(GreenButOffset)=>
   find :  Find() =S<int>(0)=>
   speech : SpeechNode("Try again")=T(1000)=> wait

   find =S<int>(1)=>
    speech2 : SpeechNode("Moving")
 //=T(2500)=> loop
//FindObstacles() =MAP=>
//exec: ExecutePath() =C=> loop

			#endstatemachine

			}

};

#endif