//-*-c++-*-
/**
 * @file WallReflectBehavior.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_WallReflectBehavior_h_
#define INCLUDED_WallReflectBehavior_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"
#include "Behaviors/Nodes/XWalkNode.h"
#include "Motion/MotionPtr.h"

using namespace DualCoding;

class WallScan : public VisualRoutinesStateNode {
public:
  WallScan() : VisualRoutinesStateNode("WallScan"), _req_id(-1),
               _dist_from_pan_to_frame(0.0) {}

    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.0005;
        req.motionSettleTime = 2000;
        // Look up IRFrame local coordinates
        fmat::Column<3> irLocation =
          kine->jointToBase(req.frameOffset).translation();
        // Get pan-to-IR-frame dist for later use
        fmat::Column<3> panLocation =
          kine->jointToBase(HeadPanOffset).translation();
        Point irPoint(irLocation, egocentric);
        Point panPoint(panLocation, egocentric);
        _dist_from_pan_to_frame = irPoint.xyDistanceFrom(panPoint);
        std::cout << "Center IR location: " << irPoint << std::endl;
        std::cout << "Head pan location: " << panPoint << std::endl;
        std::cout << "Dist between IR and Pan: " << _dist_from_pan_to_frame
                  << 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();
        float max_sensor = 0.0;
        float max_sensor_pan;
        NEW_SKETCH(graph, bool, visops::zeros(localSkS));
        for ( ; iter != end; ++iter) {
          Point &p = *iter;
          if (p.coordZ() > max_sensor) {
            max_sensor = p.coordZ();
            max_sensor_pan = p.coordX();
          }
          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();
        }
        // Output wall angle
        std::cout << "Robot must turn by angle of " << max_sensor_pan
                  << " radians to be perpendicular to wall" << std::endl;
        // Get perpendicular distance to wall
        float perp_dist = sensor_to_perp_dist(max_sensor);
        std::cout << "Perpendicular distance to wall is " << perp_dist
                  << " millimeters." << std::endl;
        // Use perp. distance to find forward distance to wall
        // Must take rotation of head into account
        float perp_dist_from_pan = perp_dist + _dist_from_pan_to_frame;
        float forward_dist_from_pan = perp_dist_from_pan / cos(max_sensor_pan);
        float forward_dist = forward_dist_from_pan - _dist_from_pan_to_frame;
        std::cout << "Forward distance to wall is " << forward_dist
                  << " millimeters" << std::endl;
        if (forward_dist <= 300.0) {
          // Post a WalkRequest to rotate away from wall
          float ang_compl = Pi / 2.0 + max_sensor_pan;
          float adisp = (max_sensor_pan < 0.0 ? 2.0 : -2.0) * ang_compl;
          WalkRequest req(0.0, 0.0, 0.0, 0.0, 0.05, adisp);
          std::cout << "Posting state signal with WalkRequest of "
                    << adisp << " radians." << std::endl;
          postStateSignal<WalkRequest>(req);
        } else {
          postStateCompletion();
        }
      }
    }
  
protected:
  float sensor_to_perp_dist(float sensor) {
    static float const a = 34.26;
    static float const b = -1.616;
    static float const c = -0.03016;
    return 10.0 * pow((sensor - c) / a, 1.0f / b);
  }

private:
  unsigned int _req_id;
  float _dist_from_pan_to_frame;
};

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

  virtual void setup() {
    // (1) Scan with Center IR, find walls, if any
    // (2) Use scan measurements to decide what to do
    //   (a) If walls are not present or far away, walk in a straight line
    //       for 10 cm.
    //   (b) If walls are <= 20 cm away, rotate so that heading "reflects" off
    //       wall
    // (3) Go to (1)
    MotionManager::MC_ID walkID = addMotion(MotionPtr<XWalkMC>());
#statemachine
  startnode: WallScan()

      walknode: XWalkNode(0.0, 200.0, 0.0, 0.0, 0.0, 0.0)[setMC(walkID);] =C=> startnode

      turnnode: XWalkNode()[setMC(walkID);] =C=> startnode

      startnode =C=> walknode

      startnode =S<WalkRequest>=> turnnode
#endstatemachine
  }
};

#endif