/*
 * autolevelChain.cpp
 *
 *  Created on: 20 Oct 2014
 *      Author: timothyw
 */

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

namespace casualty {
using namespace crosbot;

Dynamixel* AutoLevelChain::createServo(ConfigElementPtr cfg) {
	Dynamixel* rval = make(cfg);
	configure(rval, cfg);
	return rval;
}

Dynamixel* AutoLevelChain::make(ConfigElementPtr cfg) {
	return new AutoLevelServo(*this);
}

void AutoLevelChain::configure(Dynamixel* rval, ConfigElementPtr cfg) {
	if (rval != NULL) {
		rval->initialize(cfg);
		if (rval->getID() < 0) {
//			delete rval;
			rval = NULL;
		} else {
			servos[numServos] = rval;
			++numServos;
		}
	}
}

void AutoLevelServo::initialize(crosbot::ConfigElementPtr cfg) {
	std::string stype = cfg->getParam("type", "");
	//ROS_INFO("AutoLevel stype: %s", stype.c_str());
	if (strcasecmp(stype.c_str(), "roll") == 0 || strcasecmp(stype.c_str(), "r") == 0) {
		type = AutoLevelServo::AutoLevelRoll;
		min = DEG2RAD(-90); max = DEG2RAD(90);
		offset = DEG2RAD(cfg->getParamAsDouble("offset", RAD2DEG(offset)));
	} else if (strcasecmp(stype.c_str(), "tilt") == 0 || strcasecmp(stype.c_str(), "t") == 0 ||
			strcasecmp(stype.c_str(), "pitch") == 0 || strcasecmp(stype.c_str(), "p") == 0) {
		type = AutoLevelServo::AutoLevelTilt;
		min = DEG2RAD(-90); max = DEG2RAD(90);
		offset = DEG2RAD(cfg->getParamAsDouble("offset", RAD2DEG(offset)));
	} else if (strcasecmp(stype.c_str(), "scan") == 0 || strcasecmp(stype.c_str(), "s") == 0) {
		//ROS_INFO("AutoLevel: Creating scan servo");
	    type = AutoLevelServo::AutoLevelScan;
		min = DEG2RAD(-90); max = DEG2RAD(90);
		speed = cfg->getParamAsInt("speed", 50);
	} else if (strcasecmp(stype.c_str(), "constant") == 0 || strcasecmp(stype.c_str(), "c") == 0) {
		type = AutoLevelServo::Constant;
		constant = DEG2RAD(cfg->getParamAsDouble("value", RAD2DEG(constant)));
	}
	Dynamixel::initialize(cfg);

	// TODO:
	stow = DEG2RAD(cfg->getParamAsDouble("stow", RAD2DEG(stow)));
	park = DEG2RAD(cfg->getParamAsDouble("park", RAD2DEG(park)));

	ROS_INFO("AutoLevel: Have servo: %d, type: %s, min: %.1f, max(%.1f)",
			id, stype.c_str(), min, max);
	ROS_INFO("                           stow: %.1f, park: %.1f, invert: %d",
			stow, park, invert);
}

void AutoLevelServo::adjustLevel(Quaternion orient) {
	if (type == AutoLevelRoll || type == AutoLevelTilt) {
		//ROS_INFO("AutoLevel: Adjusting level servo %d", id);
		double desiredPose = 0;

		double Y,P,R;
		orient.getYPR(Y,P,R);

		if (type == AutoLevelRoll) {
			desiredPose = -R;
//			ROS_INFO("AutoLevel: Adjusting servo %d roll %lf", id, RAD2DEG(desiredPose));
		} else if (type == AutoLevelTilt) {
			desiredPose = -P;
//			ROS_INFO("AutoLevel: Adjusting servo %d tilt %lf", id, RAD2DEG(desiredPose));
		} else {
			ERROR("AutoLevel: Unknown autoleveller dxl type 0x%x.\n", type);
			return;
		}

		desiredPose += offset;
		setPos(desiredPose);
	} else  if (type == AutoLevelScan) {
		ROS_INFO("AutoLevel: Adjusting scanning servo %d", id);
		ROS_INFO("AutoLevel: \t offset: %.2lf", offset);
        double diffC = fabs(currentPose - constant + offset), diffL = fabs(lastPose - constant + offset);
        if (currentPose < min || (fabs(currentPose - min) <= DXL_POSITION_ERROR)) {
        	constant = max;
        } else if (currentPose > max || (fabs(currentPose - max) <= DXL_POSITION_ERROR)) {
        	constant = min;
        } else if (constant != min && constant != max) {
            constant = min;
        }

        lastPose = currentPose;
        setPos(constant);
	} else if (type == Constant) {
	    //ROS_INFO("AutoLevel: Adjusting constant servo %d", id);
		setPos(constant);
	}
}

} // namespace casualty

