/*
 * dynamixel2.cpp
 *
 *  Created on: 13/06/2013
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <dynamixel/dynamixel.h>

namespace casualty {
using namespace crosbot;

DynamixelChain::~DynamixelChain() {}

Dynamixel* DynamixelChain::createServo(ConfigElementPtr cfg) {
	Dynamixel* rval = new Dynamixel(*this);
	if (rval != NULL) {
		rval->initialize(cfg);

		if (rval->id < 0) {
			delete rval;
			rval = NULL;
		} else {
			servos[numServos] = rval;
			++numServos;
		}
	}
	return rval;
}

void Dynamixel::initialize(crosbot::ConfigElementPtr cfg) {
	id = cfg->getParamAsInt("id", id);

	invert = cfg->getParamAsBool("invert", invert);
	min = DEG2RAD(cfg->getParamAsDouble("min", RAD2DEG(min)));
	max = DEG2RAD(cfg->getParamAsDouble("max", RAD2DEG(max)));
	zero = cfg->getParamAsInt("zero", zero);
	zeroOnStart = cfg->getParamAsBool("zeroOnStart", zeroOnStart);
	speed = cfg->getParamAsInt("speed", speed);
}

void DynamixelChain::initialize(ConfigElementPtr cfg) {
	port = cfg->getParam("port", port);
	ROS_INFO("Dynamixel: Using DXL port: %s", port.c_str());
}

inline bool Dynamixel::readServo() {
	if (id < 0) return false;

	int tick = chain.commPort.read_pos(id);

	if (chain.commPort.get_result() == COMM_RXSUCCESS) {
		this->tick = tick;
		double pos = DXL2RAD2(tick-zero);
		currentPose = invert?-pos:pos;
//			ROS_INFO("Dynamixel: %d at pose: %.2f\n", servoIDs[i], servos[i]->getPos());
	} else {
		ROS_INFO("Dynamixel: Error reading from servo %d.(err: %d)", id, chain.commPort.get_result());
		return false;
	}

	return true;
}

void Dynamixel::connect() {
	if (id < 0) return;
//	ROS_INFO("Dynamixel2: %d connecting", id);

	chain.commPort.ping(id);
	readServo();
	if (zeroOnStart) {
//		ROS_INFO("Dynamixel2: %d zeroing at start", id);
		setPos(0);
	}
}

inline void Dynamixel::setTick(int tick, int vel) {
	chain.commPort.write_pos(id, tick, vel);
}

} // namespace casualty
