/*
 * node.cpp
 *
 *  Created on: 30/05/2013
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <emu_arm/emu_arm.hpp>
#include <crosbot/data.hpp>

#include <sensor_msgs/JointState.h>

#include <emu_arm/LookAt.h>
#include <emu_arm/Track.h>
#include <emu_arm/Scan.h>
#include <tf/transform_listener.h>

using namespace casualty;

// **********************************************************************

class ArmNode : public EmuArm {
public:
	ros::Publisher jointPub;
	ros::Subscriber jointSub;
	ros::ServiceServer lookAtSrv, trackSrv, scanSrv;
	tf::TransformListener tfListen;
	std::string robotFrame;

	geometry_msgs::PointStamped pointToTrack;
	bool scanning; double scanAngle;

	class TrackThread : public Thread {
	public:
		ArmNode& arm;
		bool operating;
		TrackThread(ArmNode& arm) : arm(arm), operating(true) {}

		void run() {
			arm.track();
		}
	};
	TrackThread trackThread;

	ArmNode() : robotFrame("base_link"),
			scanning(false), scanAngle(DEG2RAD(90)),
			trackThread(*this) {
		pointToTrack.point.x = pointToTrack.point.y = pointToTrack.point.z = INFINITY;
		trackThread.start();
	}

	void callbackJoints(sensor_msgs::JointStateConstPtr joints) {
		size_t jointCount = MIN(joints->name.size(), joints->position.size());
		for (size_t i = 0; i < jointCount; ++i) {
			if (joints->name[i] == "arm_base") {
				base.SetPosition(joints->position[i]);
				scanning = false;
			} else if (joints->name[i] == "arm_shoulder") {
				shoulder.SetPosition(joints->position[i]);
				scanning = false;
			} else if (joints->name[i] == "arm_elbow") {
				elbow.SetPosition(joints->position[i]);
				scanning = false;
			} else if (joints->name[i] == "neck_tilt" && neckTilt != NULL) {
				neckTilt->setPos(joints->position[i]);
				scanning = false;
			} else if (joints->name[i] == "neck_pan" && neckPan != NULL) {
				neckPan->setPos(joints->position[i]);
				scanning = false;
			}
		}
	}

	bool callbackTrack(emu_arm::Track::Request& req, emu_arm::Track::Response& res) {
		// Track point
		scanning = false;
		LOG("EmuArm:: callbackTrack\n");
		if (req.mode == 0)
			pointToTrack.point.x = pointToTrack.point.y = pointToTrack.point.z = INFINITY;
		else
			pointToTrack = req.point;

		return true;
	}

	bool callbackScan(emu_arm::Scan::Request& req, emu_arm::Scan::Response& res) {
		// TODO: Track move arm to stow position
		LOG("EmuArm:: callbackScan\n");

		scanning = (req.mode == 1);


		if (!scanning) {
			if (neckPan != NULL)
				neckPan->setPos(neckPan->getPos());
			return true;
		}

		double baseA = DEG2RAD(0), shoulderA = DEG2RAD(-80), elbowA = DEG2RAD(135);
		double tiltA = -(elbowA + shoulderA);
//		LOG("EmuArm:: callbackScan - tilt %.1lf\n", RAD2DEG(tiltA));

		base.SetPosition(baseA);
		shoulder.SetPosition(shoulderA);
		elbow.SetPosition(elbowA);
		if (neckTilt != NULL)
			neckTilt->setPos(tiltA);

		return true;
	}

	bool callbackLookAt(emu_arm::LookAt::Request& req, emu_arm::LookAt::Response& res) {
		Point point = req.point;

		// convert to robot relative point
		LOG("EmuArm:: callbackLookAt: %s -> %s\n",req.point.header.frame_id.c_str(), robotFrame.c_str());

		tf::StampedTransform transform;
		try {
			ros::Time now = ros::Time::now();
			tfListen.waitForTransform(robotFrame, req.point.header.frame_id, now, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM));
			tfListen.lookupTransform(robotFrame, req.point.header.frame_id, now, transform);
		} catch (tf::TransformException& ex) {
			ERROR("emu_arm: Error getting transform. (%s)\n", ex.what());
			return false;
		}

		point =  transform * point.toTF();
		LOG("callbackLookAt: (%.3lf, %.3lf, %.3lf)\n", point.x, point.y, point.z);
//		point = transform.
		return lookAtObject(point, robotFrame);
	}

	bool getNeckAngles(Point point, const std::string& frame, double& tilt, double& pan) {
		tf::StampedTransform transform;
		try {
			ros::Time now = ros::Time::now();
			tfListen.waitForTransform("/wrist", frame, now, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM));
			tfListen.lookupTransform("/wrist", frame, now, transform);
		} catch (tf::TransformException& ex) {
			ERROR("emu_arm: Error getting transform. (%s)\n", ex.what());
			return false;
		}

		point = transform * point.toTF();

		tilt = atan2(-point.z, sqrt(point.x*point.x+point.y*point.y)),
				pan = atan2(point.y, sqrt(point.z*point.z+point.x*point.x));
      
      if (point.x < 0) {
          tilt = -tilt;
          pan = M_PI - pan;
      }

		return true;
	}

	void track() {
		double scanPos = 0, previousPos;
		ros::Time lastMove = ros::Time::now();

		while (trackThread.operating) {
			usleep(10000);

			if (scanning) {
				if (neckPan == NULL)
					continue;
//				if (neckTilt != NULL)
//					neckTilt->setPos(-(elbow.GetPosition() + shoulder.GetPosition()));
//				LOG("EmuArm:: Scanning.\n");
				double pos = neckPan->getPos();

				ros::Time now = ros::Time::now();
				if (fabs(scanPos - pos) < fabs(scanPos - previousPos)) {
					lastMove = ros::Time::now();
					previousPos = pos;
				}
				if (scanPos == -scanAngle && (pos < scanPos || fabs(scanPos - pos) < 0.03)) {
//					LOG("EmuArm::track() - negative limit.\n");
					scanPos = scanAngle;
					lastMove = now;
				} else if (scanPos != -scanAngle && (pos >= scanPos || fabs(scanPos - pos) < 0.03)) {
//					LOG("EmuArm::track() - positive limit.\n");
					scanPos = -scanAngle;
					lastMove = now;
				} else if ((now - lastMove).toSec() > 1) {
//					LOG("EmuArm::track() - timeout. (%lf)\n", fabs(scanPos - pos));
					if (scanPos < 0) {
						scanPos = scanAngle;
					} else {
						scanPos = -scanAngle;
					}
					lastMove = now;
				} else {
//					LOG("EmuArm::track() - %lf, %lf\n", fabs(scanPos - pos), (now - lastMove).toSec());
				}


				neckPan->setPos(scanPos, M_PI / 5);
				continue;
			}

			scanPos = 0;
			lastMove = ros::Time::now();

			geometry_msgs::PointStamped pointS = pointToTrack;
			Point point = pointS.point;

			if (!point.isFinite()) {
				continue;
			}

			tf::StampedTransform transform;
			try {
				ros::Time now = ros::Time::now();
				tfListen.waitForTransform("/wrist", pointS.header.frame_id, now, ros::Duration(1.0));
				tfListen.lookupTransform("/wrist", pointS.header.frame_id, now, transform);
			} catch (tf::TransformException& ex) {
				ERROR("emu_arm: Error getting transform. (%s)\n", ex.what());
				continue;
			}

			point = transform * point.toTF();

//         LOG("EmuArm: x %.3lf y %.3lf z %.3lf\n", point.x, point.y, point.z);

			double tilt = atan2(-point.z, sqrt(point.x*point.x+point.y*point.y)),
					pan = atan2(point.y, sqrt(point.z*point.z+point.x*point.x));
         
         if (point.x < 0) {
            tilt = -tilt;
            pan = M_PI - pan;
         }

//         LOG("EmuArm: tilt %.1lf pan %.1lf\n", RAD2DEG(tilt), RAD2DEG(pan));

			if (neckTilt != NULL && neckPan != NULL) {
				neckTilt->setPos(tilt);
				neckPan->setPos(pan);
			}
		}
	}

	virtual void updatePoses() {
		sensor_msgs::JointState jstate;
		jstate.header.stamp = ros::Time::now();

		jstate.name.push_back("arm_base");
		jstate.position.push_back(base.GetPosition());
		jstate.name.push_back("arm_shoulder");
		jstate.position.push_back(shoulder.GetPosition());
		jstate.name.push_back("arm_elbow");
		jstate.position.push_back(elbow.GetPosition());

		jstate.name.push_back("neck_tilt");
		jstate.position.push_back(neckTilt != NULL?neckTilt->getPos():0);
		jstate.name.push_back("neck_pan");
		jstate.position.push_back(neckPan != NULL?neckPan->getPos():0);

		jointPub.publish(jstate);
	}

	virtual void Shutdown() {
		trackThread.operating = false;
		jointPub.shutdown();
		jointSub.shutdown();

		EmuArm::Shutdown();
	}

	Pose getViewPoint(Point object, Point vector) {
		Point groundObject(object.x, object.y, 0), groundVec(vector.x, vector.y, 0), unitVec = vector.normalise();

		double cosTheta = groundObject.normalise().dot(groundVec.normalise());
		Point groundApproach = groundObject - (groundVec.normalise() * cosTheta * groundObject.length());

//		double d1 = (groundApproach - groundObject).length(), d2 = (vector.length());

		double zk1 = object.z + unitVec.z * (groundApproach.x - object.x) / unitVec.x,
				zk2 = object.z + unitVec.z * (groundApproach.y - object.y) / unitVec.y ;
		LOG("getViewPoint: z1: %.1lf, z2: %.1lf\n", zk1, zk2);
		Point viewpoint(groundApproach.x, groundApproach.y, zk1);
		if (unitVec.x == 0)
			viewpoint.z = zk2;

		double dx = object.x - viewpoint.x, dy = object.y - viewpoint.y;
		double dxy = sqrt(dx*dx+dy*dy);
		double yaw = acos(dx / dxy), pitch = atan((viewpoint.z - object.z) / dxy);

		Pose rval;
		rval.position = viewpoint;
		rval.orientation.setYPR(yaw, pitch, 0);
		return rval;
	}
};

int main(int argc, char **argv) {
	ros::init(argc, argv, "emu_arm");
	ArmNode arm;

//	Point point(1,1,1), vector(-2, -1, 0);
//	Pose view = arm.getViewPoint(point, vector);
//	LOG("Arm: For point (%.1lf, %.1lf, %.1lf) on vector (%.1lf, %.1lf, %.1lf)\n",
//			point.x, point.y, point.z, vector.x, vector.y, vector.z);
//	LOG("Arm: Best view point is (%.1lf, %.1lf, %.1lf)\n",
//			view.position.x, view.position.y, view.position.z);
//	double r,p,y;
//	view.getYPR(y,p,r);
//	LOG("Arm: Best view angle is (%.1lf, %.1lf, %.1lf)\n",
//					RAD2DEG(r), RAD2DEG(p), RAD2DEG(y));

	ros::NodeHandle node;
	arm.jointPub = node.advertise<sensor_msgs::JointState>("joint_states", 1);
	arm.jointSub = node.subscribe("joint_control", 1, &ArmNode::callbackJoints, &arm);

	arm.lookAtSrv = node.advertiseService("/emu_arm/look_at", &ArmNode::callbackLookAt, &arm);
	arm.trackSrv = node.advertiseService("/emu_arm/track_object", &ArmNode::callbackTrack, &arm);
	arm.scanSrv = node.advertiseService("/emu_arm/scan", &ArmNode::callbackScan, &arm);

	crosbot::ConfigElementPtr cfg = new crosbot::ROSConfigElement(ros::NodeHandle("~"));
	arm.Initialize(cfg);

	while (ros::ok()) {
		ros::spin();
	}

	// shutdown arm
	arm.Shutdown();

	return 0;
}

