/*
 * autolevel.cpp
 *
 *  Created on: 06/09/2012
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <autolevel/autolevel.hpp>

#include <crosbot/data.hpp>
#include <dynamixel/dynamixel.h>

#define MAX(X, Y)	((X)<(Y)?(Y):(X))

namespace casualty {

AutoLevel::AutoLevel() :
	operating(false),
	xsensFactory(new DefaultFactoryXSens()), autolevelChainFactory(new DefaultFactoryAutoLevelChain()),
	state(Unknown),
	stowAngle(DEG2RAD(45)), unstowAngle(DEG2RAD(45)), stowErr(DEG2RAD(5)),
//	nextStowPoint(0), parkLocation(NULL),
	tiltServo(NULL), rollServo(NULL), scanServo(NULL),
	mtiThread(*this), dxlThread(*this)
{}

AutoLevel::AutoLevel(FactoryXSensPtr xsensFactory, FactoryAutoLevelChainPtr autolevelChainFactory) :
	AutoLevel()
{
	this->xsensFactory = xsensFactory;
	this->autolevelChainFactory = autolevelChainFactory;
}

AutoLevel::~AutoLevel() {
	stop();
}

void AutoLevel::configure(ConfigElementPtr config) {
	stowAngle = DEG2RAD(config->getParamAsDouble("stow", RAD2DEG(stowAngle)));
	unstowAngle = DEG2RAD(config->getParamAsDouble("unstow", RAD2DEG(unstowAngle)));
	if (unstowAngle > stowAngle)
		unstowAngle = stowAngle;

	// Create and configure
	xsens = xsensFactory->makeXSens();
	ConfigElementPtr xsensConfig = config->getChild("xsens");
	if (xsensConfig != NULL) {
		std::string port;
		bool resetOnStart;
		port 		 = xsensConfig->getParam("port", xsens->getPort());
		resetOnStart = xsensConfig->getParamAsBool("reset", xsens->getResetOnStart());
		xsens->setPort(port);
		xsens->setResetOnStart(resetOnStart);
	}

	// Create AutoLevelChain
	dxlChain = autolevelChainFactory->makeAutoLevelChain();

	// Configure AutoLevelChain
	ConfigElementPtr dxlConfig = config->getChild("dxl");
	if (dxlConfig != NULL) {
		dxlChain->initialize(dxlConfig);
//		dxlChain->port = dxlConfig->getParam("port", dxlChain->port);

//		ROS_INFO("AutoLevel: Using DXL port: %s", dxlChain.port.c_str());
		for (uint32_t i = 0; i < DXL_MAXSERVOS; ++i) {
			char cStr[256];
			sprintf(cStr, "servo%d", i);

			std::string servoName(cStr);

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

			Dynamixel* servo = dxlChain->createServo(servoConfig);
			if (servo == NULL || servo->getID() < 0)
				break;
		}

		AutoLevelServo* servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(0));
		if (servo != NULL) {
		    if (servo->getType() == AutoLevelServo::AutoLevelTilt)
		        tiltServo = servo;
            else if (servo->getType() == AutoLevelServo::AutoLevelRoll)
                rollServo = servo;
		}
		servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(1));
        if (servo != NULL) {
            if (servo->getType() == AutoLevelServo::AutoLevelTilt)
                tiltServo = servo;
            else if (servo->getType() == AutoLevelServo::AutoLevelRoll)
                rollServo = servo;
        }
        if (tiltServo == NULL)
            tiltServo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(0));
		if (rollServo == NULL)
		    rollServo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(1));
		scanServo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(2));
	}
}

void AutoLevel::start() {
	if (operating)
		return;

	operating = true;

	mtiThread.start();
	dxlThread.start();
}

void AutoLevel::stop() {
	operating = false;

	for (uint32_t i = 0; i < DEFAULT_WAIT_FOR_THREAD_CLOSE && (mtiThread.isAlive() || dxlThread.isAlive()); i += 10) {
		usleep(10000);
	}

	if (mtiThread.isAlive()) {
		ERROR("AutoLevel: MTi thread didn't close cleanly.\n");
	}
	if (dxlThread.isAlive()) {
		ERROR("AutoLevel: Dynamixel thread didn't close cleanly.\n");
	}
}

void AutoLevel::mtiThreadBody() {
	//  open xSens
	xsens->connect();

	while (operating) {
		// read xSens values
//		tf::Quaternion tf = ;
		Quaternion orientation = Quaternion(xsens->getOrientation());

		updateOrientation(orientation);

		// update servo positions
		updateServos(orientation);
		usleep(1000);
	}

	// close xSens
	ROS_INFO("XSens: Disconnecting from XSens");
	xsens->disconnect();
}

void AutoLevel::dxlThreadBody() {
	dxlChain->connect();

	while (operating) {
		dxlChain->readServos();

		// TODO: calculate pose

		usleep(1000);
	}

	park();

	dxlChain->disconnect();
}

void AutoLevel::updateServos(Quaternion orientation) {
	if (state == Parking)
		return;
	double y, p, r;
	orientation.getYPR(y,p,r);

    //ROS_INFO("Adjusting servos. (Pitch: %.1f, Roll: %.1f) state - %d",
    //        RAD2DEG(p), RAD2DEG(r), state);
	if (state == Ready && fabs(p) < stowAngle && fabs(r) < stowAngle) {
		for (uint32_t i = 0; i < dxlChain->getNumServos(); ++i) {
        	AutoLevelServo* servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(i));
			// TODO: Do smarter
			servo->adjustLevel(orientation);
		}
	} else if (fabs(p) < unstowAngle && fabs(r) < unstowAngle) {
		unstow();
	} else {
        //ROS_INFO("AutoLevel: Stowing (%.1lf).", RAD2DEG(MAX(fabs(r), fabs(p))));
		stow();
	}

	double tilt = (tiltServo != NULL? tiltServo->getPos(): 0),
			roll = (rollServo != NULL? rollServo->getPos(): 0),
			scan = (scanServo != NULL? scanServo->getPos(): 0);
	updateJoints(tilt, roll, scan);
}

void AutoLevel::stow() {
	if (state != Stowing) {
        ROS_INFO("AutoLevel: Stowing.");
        state = Stowing;
        std::vector< double > stowPoint;
        for (uint32_t i = 0; i < dxlChain->getNumServos(); ++i) {
        	AutoLevelServo* servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(i));
        	stowPoint.push_back(servo->getStow());
        }
        gotoStowPoint(stowPoint);
	}
}

void AutoLevel::unstow() {
    std::vector< double > stowPoint;
    for (uint32_t i = 0; i < dxlChain->getNumServos(); ++i) {
    	AutoLevelServo* servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(i));
        if (servo->getType() == AutoLevelServo::Constant) {
            stowPoint.push_back(servo->getConstant());
        } else {
            stowPoint.push_back(0);
        }
    }

	if (state != Unstowing) {
        ROS_INFO("AutoLevel: Unstowing");
		state = Unstowing;
        gotoStowPoint(stowPoint);
	} else {
	    if (atStowPoint(stowPoint)) {
	        state = Ready;
	    }
	}
}

void AutoLevel::park() {
	state = Parking;
    ROS_INFO("AutoLevel: Parking");

	bool moving = true;

    // goto park location
	std::vector< double >* parkLocation = new std::vector< double>();
	int numFinite = 0;
	for (uint32_t i = 0; i < dxlChain->getNumServos(); ++i) {
		AutoLevelServo* servo = dynamic_cast<AutoLevelServo*>(dxlChain->getServo(i));
		parkLocation->push_back(servo->getPark());
		if (servo->getPark() != INFINITY)
			numFinite++;
	}
	if (numFinite == 0)
		moving = false;

	double distanceToPoint = INFINITY;
	Time lastMoved = Time::now();

	while (moving) {
		dxlChain->readServos();
		if (atStowPoint(*parkLocation)) {
			break;
		}

		gotoStowPoint(*parkLocation);

		usleep(10000);

		// find distance to point
		double d = 0;
		for (unsigned int i = 0; i < MIN(parkLocation->size(), dxlChain->getNumServos()); i++) {
			d += fabs((*parkLocation)[i] - dxlChain->getServo(i)->getPos());
		}

		if (d < distanceToPoint) {
			lastMoved = Time::now();
			distanceToPoint = d;
		} else {
			long timeFromLastMove = (Time::now() - lastMoved).toSec();

			// If servos haven't moved closer to point in 5 seconds, they're no longer moving
			if (timeFromLastMove >= 5) {
				ERROR("AutoLevel: Servos have stopped moving to park location.\n");
				moving = false;
			}
		}
	}
}

//int AutoLevel::getNearestStowPoint() {
//	int nearest = -1;
//	double nearestDiff = INFINITY;
//	for (unsigned int i = 0; i < stowPoints.size(); i++) {
//		double diff = 0;
//
//		unsigned int minJ = MIN(stowPoints[i].size(), dxlChain->numServos);
//		for (unsigned int j = 0; j < minJ; j++) {
//			diff += fabs(dxlChain->servos[j]->getPos()-stowPoints[i][j]);
//		}
//
//		if (diff < nearestDiff) {
//			nearestDiff = diff;
//			nearest = i;
//		}
//	}
//
//	return nearest;
//}

} // namespace casualty


