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

#include "DualCoding/DualCoding.h"
#include "Behaviors/Demos/Tapia/TapiaMarkerData.h"

using namespace DualCoding;

class ProjectionTest : public VisualRoutinesBehavior {
public:
  ProjectionTest() : VisualRoutinesBehavior("ProjectionTest") {}

  virtual void DoStart() {
    VisualRoutinesBehavior::DoStart();

    cout << "cam to base " << kine->jointToBase(CameraFrameOffset) << endl;

    const int pink_index = ProjectInterface::getColorIndex("pink");    
    const int blue_index = ProjectInterface::getColorIndex("blue");
    const int orange_index = ProjectInterface::getColorIndex("orange");

    MapBuilder::setVerbosity(-1u);

    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.maxDist = 100000;
    req.rawY = true;
    req.clearShapes = false;

    req.markerTypes.insert(TapiaMarkerData::tapiaMarkerType);
    
    req.objectColors[markerDataType].insert(pink_index);
    req.objectColors[markerDataType].insert(blue_index);
    req.objectColors[markerDataType].insert(orange_index);

    unsigned int mapreq_id = mapBuilder.executeRequest(req);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
  }
  
  virtual void processEvent(const EventBase &event) {
    cout << "MapBuilder returned " << event.getDescription() << endl;
    
    // project to local space
    cout << "Found " << localShS.allShapes().size() << " local shapes." << endl;

    SHAPEROOTVEC_ITERATE(localShS, s)
      if (s->isType(markerDataType)) {
	const Shape<MarkerData>& m = ShapeRootTypeConst(s,MarkerData);
	projectMarkerToCam(m);
      }
    END_ITERATE;
  }

  virtual void projectMarkerToCam(const Shape<MarkerData>& m)
  {
    cout << "Marker: " << m->getCentroid() << endl;
    
    // get centroid
    Point c = m->getCentroid();
    
    // transform to camera coordinates
    c.applyTransform(kine->baseToJoint(CameraFrameOffset));
    
    cout << "Transformed center: " << c << endl;
    
    // compute pixels
    float cx, cy;
    config->vision.computePixel(c.coordX(), c.coordY(), c.coordZ(), cx, cy);
    
    cout << "Pixel pre: " << cx << "," << cy << endl;
    
    cx = (cx + 1)/2*VRmixin::camSkS.getWidth();
    cy = (cy + 1)/2*VRmixin::camSkS.getHeight();
    
    cout << "Pixel post: " << cx << "," << cy << endl;
    
    NEW_SHAPE(projected, MarkerData, new MarkerData(camShS, Point(cx, cy)));
  }
};

#endif