/*
 * emu_arm.cpp
 *
 *  Created on: 05/06/2013
 *      Author: mmcgill
 */

#include <ros/ros.h>

#include <emu_arm/emu_arm.hpp>

#include <sensor_msgs/JointState.h>
//#include <cmath>

namespace casualty {
//using namespace std;

// TODO: other polulus
void EmuArm::Initialize(crosbot::ConfigElementPtr cfg) {

	ConfigElementPtr baseConfig = cfg->getChild("base"),
			shoulderConfig = cfg->getChild("shoulder"),
			elbowConfig = cfg->getChild("elbow");
	base.Initialize(baseConfig);
	shoulder.Initialize(shoulderConfig);
	elbow.Initialize(elbowConfig);

	ConfigElementPtr neckCfg = cfg->getChild("neck");
	if (neckCfg != NULL) {
		neck.initialize(neckCfg);
//		neck.port = neckCfg->getParam("port", neck.port);

//		LOG("EmuArm: Using DXL port: %s\n", neck.port.c_sgr	tr());
		for (uint32_t i = 0; i < DXL_MAXSERVOS; ++i) {
			char cStr[256];
			sprintf(cStr, "servo%d", i);

			std::string servoName(cStr);

//			LOG("AutoLevel: Looking for servo: %s\n", servoName.c_str());
			ConfigElementPtr servoConfig = neckCfg->getChild(servoName);
			if (servoConfig == NULL)
				break;

			Dynamixel* servo = neck.createServo(servoConfig);
			if (servo == NULL || servo->getID() < 0)
				break;
			if (servoConfig->getParamAsBool("pan", false))
				neckPan = servo;
			else if (servoConfig->getParamAsBool("tilt", false))
				neckTilt = servo;
		}
	}

//	neck.connect();
	dxlThread.start();
}


void EmuArm::dxlThreadBody() {
	neck.connect();

	while (dxlThread.operating) {
		neck.readServos();

		updatePoses();

		usleep(10000);
	}

//	park();

	neck.disconnect();
}

double viewDistance = 0.3,
		lowerArm = 0.335, upperArm = 0.37;

Point armBase(0.145, 0, 0.19), eyeNeck(0.05, 0, 0.09);


bool EmuArm::lookAtObject(Point p, const std::string& frame) {
	LOG("EmuArm: Looking at (%.3lf, %.3lf, %.3lf) in frame %s\n", p.x, p.y, p.z, frame.c_str());
	Point objectFromShoulder = p - armBase;
	LOG("EmuArm: Looking at2 (%.3lf, %.3lf, %.3lf)\n", objectFromShoulder.x, objectFromShoulder.y, objectFromShoulder.z);
	double horizontalDistanceToObject = sqrt(objectFromShoulder.x*objectFromShoulder.x+objectFromShoulder.y*objectFromShoulder.y);

	double panA = 0, tiltA = INFINITY, elbowA = INFINITY, shoulderA = INFINITY, baseA = INFINITY;
	baseA = atan2(objectFromShoulder.y, objectFromShoulder.x);

	if (objectFromShoulder.z > (lowerArm + upperArm + eyeNeck.z)) {
		LOG("EmuArm: Looking at high object.\n");
		// Zero arm and tilt head
		shoulderA = 0; elbowA = 0;
		tiltA = -atan2(objectFromShoulder.z - (lowerArm + upperArm), horizontalDistanceToObject);
//	} else if (objectFromShoulder.z >= 0) {
	} else {
		LOG("EmuArm: Looking at object.\n");
		Point desiredWrist;
		desiredWrist.z = objectFromShoulder.z - eyeNeck.z;
		double horizontalScale = (horizontalDistanceToObject - (viewDistance + eyeNeck.x)) / horizontalDistanceToObject;
		desiredWrist.x = objectFromShoulder.x * horizontalScale;
		desiredWrist.y = objectFromShoulder.y * horizontalScale;

		double desiredArmExtension = desiredWrist.length();
		if (desiredArmExtension > (lowerArm + upperArm)) {
			// TODO: Need to extend arm to full length
			LOG("EmuArm: Looking at distant object.\n");
			shoulderA = atan2(horizontalDistanceToObject - (viewDistance + eyeNeck.x), objectFromShoulder.z);
			elbowA = 0;
		} else {
			LOG("EmuArm: Looking at nearby object.\n");
			double thetaE = acos(((upperArm*upperArm + lowerArm*lowerArm) - desiredArmExtension*desiredArmExtension) / (2*lowerArm*upperArm));
			elbowA = M_PI - thetaE;

			double thetaS =
					acos(((desiredArmExtension*desiredArmExtension + upperArm*upperArm) - lowerArm*lowerArm) / (2*desiredArmExtension*upperArm));
			shoulderA = M_PI_2 - (thetaS + atan2(objectFromShoulder.z, horizontalDistanceToObject - (viewDistance + eyeNeck.x)));
		}
		tiltA = (M_PI - elbowA - shoulderA) - M_PI;
		NORMALISE_ANGLE(tiltA);
//		tiltA =  -(shoulderA + elbowA), panA = 0;
	}

	if (!(std::isfinite(panA) && std::isfinite(tiltA) && std::isfinite(elbowA) && std::isfinite(shoulderA) && std::isfinite(baseA)) ||
			fabs(tiltA) > M_PI_2) {
		// TODO: Raise arm until tilt = 90 degrees
		Point elbowPoint(sin(shoulderA) * upperArm, 0 , cos(shoulderA) * upperArm),
				wristFromElbow(sin(shoulderA+elbowA) * lowerArm, 0 , cos(shoulderA+elbowA) * lowerArm);

		shoulderA = acos(objectFromShoulder.z / upperArm);
		if (shoulderA > M_PI_2)
			shoulderA = M_PI_2;
		if (horizontalDistanceToObject < sin(shoulderA)*upperArm + viewDistance) {
			shoulderA = -shoulderA;
		}
		elbowA = M_PI_2 - shoulderA;
		tiltA = -M_PI_2;

//		// try math upside down
//		baseA = baseA - M_PI;
//		NORMALISE_ANGLE(baseA);
//
//		Point desiredWrist;
//		desiredWrist.z = MAX(objectFromShoulder.z + eyeNeck.z, 0.1);
//
//		double horizontalScale = (horizontalDistanceToObject - (viewDistance + eyeNeck.x)) / horizontalDistanceToObject;
//		desiredWrist.x = objectFromShoulder.x * horizontalScale;
//		desiredWrist.y = objectFromShoulder.y * horizontalScale;
//
//		double desiredArmExtension = desiredWrist.length();
//		if (desiredArmExtension > (lowerArm + upperArm)) {
//			// TODO: Need to extend arm to full length
//			LOG("EmuArm: Looking at distant low object.\n");
//			shoulderA = -atan2(horizontalDistanceToObject - (viewDistance + eyeNeck.x), objectFromShoulder.z);
//			elbowA = 0;
//		} else {
//			LOG("EmuArm: Looking at nearby low object.\n");
//			double thetaE = acos(((upperArm*upperArm + lowerArm*lowerArm) - desiredArmExtension*desiredArmExtension) / (2*lowerArm*upperArm));
//			elbowA = thetaE - M_PI;
//
//			double thetaS =
//					acos(((desiredArmExtension*desiredArmExtension + upperArm*upperArm) - lowerArm*lowerArm) / (2*desiredArmExtension*upperArm));
//			shoulderA = -(M_PI_2 - (thetaS + atan2(objectFromShoulder.z, horizontalDistanceToObject - (viewDistance + eyeNeck.x))));
//		}
//		tiltA = M_PI - (shoulderA + elbowA);
//		NORMALISE_ANGLE(tiltA);
	}

	// TODO: Avoid contact with laser/autolevel
	if (fabs(baseA - M_PI_2) < DEG2RAD(15)) {
		if (shoulderA > 0) {
			shoulderA = 0;
		}
		if (elbowA + shoulderA > M_PI_2) {
			elbowA = M_PI_2 - shoulderA;
		}
	} else if (fabs(baseA + M_PI_2) < DEG2RAD(15)) {
		if (shoulderA < 0) {
			shoulderA = 0;
		}
		if (elbowA + shoulderA < -M_PI_2) {
			elbowA = -M_PI_2 - shoulderA;
		}
	}


	if (std::isfinite(panA) && std::isfinite(tiltA) && std::isfinite(elbowA) && std::isfinite(shoulderA) && std::isfinite(baseA)) {
		LOG("EmuArm: Have some arm angles\n");
		LOG("        base %.1lf shoulder %.1lf elbow %.1lf\n", RAD2DEG(baseA), RAD2DEG(shoulderA), RAD2DEG(elbowA));
		LOG("        tilt %.1lf pan %.1lf\n", RAD2DEG(tiltA), RAD2DEG(panA));
		LOG("        base %.1lf shoulder %.1lf elbow %.1lf\n", (baseA), (shoulderA), (elbowA));
		LOG("        tilt %.1lf pan %.1lf\n", (tiltA), (panA));

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

		// TODO: wait for arm to move

		ros::Time lastMove = ros::Time::now(), now = ros::Time::now();;
		double distanceAtLastCheck = INFINITY;

		while (distanceAtLastCheck > 0.01 && (now - lastMove).toSec() < 2) {
			usleep(50000);

			double d = fabs(baseA - base.GetPosition()) +
					fabs(shoulderA - shoulder.GetPosition()) +
					fabs(elbowA - elbow.GetPosition());
			if (neckTilt != NULL)
				d += neckTilt->getPos();
			if (neckPan != NULL)
				d += neckPan->getPos();

			now = ros::Time::now();
			if (d < distanceAtLastCheck) {
				distanceAtLastCheck = d;
				lastMove = now;
			}
		}

		double t2, p2;
		if (getNeckAngles(p, frame, t2, p2)) {
			tiltA = t2; panA = p2;
		}

		if (distanceAtLastCheck < 0.01) {
			LOG("EmuArm: Looking at object.\n");
			return true;
		}

		ERROR("emu_arm: Failed to move to desired position.\n");
		return false;
	} else {
		ERROR("EmuArm: Bad arm angles: %lf, %lf, %lf, %lf, %lf\n", baseA, shoulderA, elbowA, tiltA, panA);
		return false;
	}
}

} // namespace casualty
