//-*-c++-*-
/**
 * @file IRScanBehavior.h
 * @brief Uses fthe Lookout to scan an area and record IR sensor
 * readings to file.
 * @author Charles Ruhland <cruhland@cmu.edu>
 * @author Jin Su Kim <jinsuk@andrew.cmu.edu>
 */

#ifndef INCLUDED_IRScanBehavior_h_
#define INCLUDED_IRScanBehavior_h_

#include <vector>
#include <iostream>
#include <fstream>
#include <cmath>

#include "Shared/Measures.h"
#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "DualCoding/LookoutRequests.h"
#include "Events/LookoutEvents.h"

using namespace DualCoding;

class RunScan : public VisualRoutinesStateNode {
public:
  RunScan() : VisualRoutinesStateNode("RunScan"), _req_id(-1) {}

    virtual void DoStart() {
        erouter->addListener(this, EventBase::lookoutEGID);

        LookoutRequest::RawIRTask raw_ir_task(0.03125);
        LookoutScanRequest req(raw_ir_task);
        req.setHeadMotionType(LookoutRequest::scan);
        req.setResultType(LookoutRequest::interestPoints);
        req.frameOffset = IRFrameOffset;
        req.scanSpeed = 0.0001;
        req.motionSettleTime = 5000;
        // Look up IRFrame local coordinates
        fmat::Column<3> irLocation =
          kine->jointToBase(req.frameOffset).translation();
        std::cout << "Center IR location: " << irLocation << std::endl;
        float x_inc = 100.0;
        float y_val = x_inc * std::tan(1.25);  // Appox. 75 degree angle
        Point left(irLocation[0] + x_inc, y_val, irLocation[2]);
        Point right(irLocation[0] + x_inc, -y_val, irLocation[2]);
        std::cout << "Left endpoint: " << left << std::endl;
        std::cout << "Right endpoint: " << right << std::endl;
        NEW_SHAPE_N(searchLine, LineData, new LineData(localShS, left, right));
        req.searchArea = searchLine;
        _req_id = getLookout().executeRequest(req);
    }

    virtual void processEvent(EventBase const &event) {
      if (event.getSourceID() == _req_id &&
          event.getTypeID() == EventBase::deactivateETID) {
        LookoutScanEvent const &lse =
          dynamic_cast<LookoutScanEvent const &>(event);
        std::vector<Point> data = lse.getTasks().front()->data;
        std::vector<Point>::iterator iter = data.begin();
        std::vector<Point>::iterator end = data.end();
        std::fstream outfile("ir-scan.txt", fstream::out);
        outfile << "Head:Pan\tHead:Tilt\tCenterIR\n";
        NEW_SKETCH(graph, bool, visops::zeros(localSkS));
        for ( ; iter != end; ++iter) {
          Point &p = *iter;
          outfile << p.coordX() << "\t\t" << p.coordY() << "\t\t" << p.coordZ()
                  << std::endl;

          Point plot(p.coordZ() * 200.0, (p.coordX() + 2.0) * 50.0, 0.0);
          NEW_SHAPE_N(pd, PointData, new PointData(localShS, plot));
          graph |= pd->getRendering();
        }
        postStateCompletion();
      }
    }
  
private:
    unsigned int _req_id;
};

class IRScanBehavior : public StateNode {
public:
  IRScanBehavior() : StateNode("IRScanBehavior") {}

  virtual void setup() {
#statemachine
  startnode: StateNode =B(GreenButOffset)=> RunScan() =C=> startnode
#endstatemachine
  }
};

#endif