//-*-c++-*-
/**
 * @file WallDemoBehavior.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_WallDemoBehavior_h_
#define INCLUDED_WallDemoBehavior_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"
#include "ForwardDistance.h"

using namespace DualCoding;

static float current_perp_pan = 0.0;
static float current_perp_dist = 0.0;
static MotionPtr<XWalkMC> walker;


class WallDemoScan : public VisualRoutinesStateNode {
public:
  WallDemoScan() : VisualRoutinesStateNode("WallDemoScan"), _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();
        }
				*/
				ForwardDistance fwd_dist;
				fwd_dist.calculate(data, _dist_from_pan_to_frame);
        // Output wall angle
        std::cout << "Robot must turn by angle of " << fwd_dist.pan_angle
                  << " radians to be perpendicular to wall" << std::endl;
				current_perp_pan = fwd_dist.pan_angle;
				current_perp_dist = fwd_dist.p_distance;
        std::cout << "Perpendicular distance to wall is "
									<< fwd_dist.p_distance << " millimeters." << std::endl;
        std::cout << "Forward distance to wall is " << fwd_dist.f_distance
                  << " millimeters" << std::endl;
        if (fwd_dist.f_distance <= 300.0) {
          // Post a WalkRequest to rotate parallel to wall
          float right_angle = Pi / 2.0;
          float adisp = fwd_dist.pan_angle +
						(fwd_dist.pan_angle < 0.0 ? right_angle : -right_angle);
          WalkRequest req(0.0, 0.0, 0.0, 0.0, 0.0, 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 WallStrafe : public XWalkNode {
public:
	WallStrafe() : XWalkNode("WallStrafe") {}

	virtual void DoStart() {
		float move_dist = current_perp_dist - 150.0;
		getMC()->setTargetDisplacement(0.0,
																	 current_perp_pan < 0.0 ? -move_dist
																	 : move_dist,
																	 0.0);
	}
};

class WallExit : public XWalkNode {
public:
	WallExit() : XWalkNode("WallExit") {}

	virtual void DoStart() {
		getMC()->setTargetDisplacement(0.0, 0.0,
																	 current_perp_pan < 0.0 ? Pi / 2 : -Pi / 2);
	}
};

class StandHeight : public StateNode {
public:
	StandHeight(float const groundOffset=-10) : StateNode("StandHeight"),
																				 _groundOffset(groundOffset) {}
	virtual void DoStart() {
		walker->groundPlane[3] = _groundOffset;
	}
	float _groundOffset;
};

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

  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 close enough, then:
		//     (i) Rotate so that robot is parallel to wall
		//     (ii) Strafe towards wall so that the robot is almost touching it
		//     (iii) Do some kind of dance/happy behavior ??
		//     (iv) Rotate to be perpendicular to wall
    // (3) Go to (1)
    MotionManager::MC_ID walkID = addMotion(walker);
#statemachine
  startnode: WallDemoScan()

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

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

      strafenode: WallStrafe()[setMC(walkID);] =C=> exitnode
      
      exitnode: WallExit()[setMC(walkID);] =C=> startnode

      startnode =C=> walknode

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

#endif