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

#ifndef CASUALTY_DYNAMIXEL2_H_
#define CASUALTY_DYNAMIXEL2_H_

#include <dynamixel/DXLComm.h>
#include <crosbot/config.hpp>
#include <crosbot/utils.hpp>

namespace casualty {
using namespace crosbot;

#define DXL2RAD2(DX)		((DX) * DX2RADFACTOR)
#define RAD2DXL2(RAD)		((RAD) / DX2RADFACTOR)

#define DEFAULT_DXL_PORT "/dev/ttyUSB0"
#define DXL_MAXSERVOS		256
#define DXL_DEFAULT_DELAY	10
#define DXL_POSITION_ERROR	0.015

class DynamixelChain;
class Dynamixel {
protected:
    DynamixelChain& chain;
	int id;
	bool invert, zeroOnStart;

	int zero, speed, tick;
	double min, max;

    double currentPose, lastPose;

    friend class DynamixelChain;
public:
    Dynamixel(DynamixelChain& chain) :
    	chain(chain), id(-1), invert(false), tick(0), zeroOnStart(false),
    	zero(512), speed(192), min(-INFINITY), max(INFINITY),
    	currentPose(INFINITY), lastPose(INFINITY)
    {}
    virtual ~Dynamixel() {}

    virtual void initialize(crosbot::ConfigElementPtr cfg);
    virtual bool readServo();
    virtual void connect();
    virtual void disconnect() {};

    inline int getID() const { return id; }

    inline double getPos(int tick) const {
    	double rval = DXL2RAD2(tick-zero);
    	if (invert) rval = -rval;
    	return rval;
    }

    inline double getPos() const {
    	return currentPose;
    }

    inline int getTick(double pos) const {
    	if (invert) pos = -pos;
    	return RAD2DXL2(pos)+zero;
    }

    virtual void setTick(int tick, int vel);

    virtual inline void setPos(double pos, double vel) {
    	setTick(getTick(pos), RAD2DXLSPEED(vel));
    }

    virtual inline void setPos(double pos) {
    	setTick(getTick(pos), speed);
    }
};

class DynamixelChain {
protected:
	std::string port;
	unsigned int numServos;
	Dynamixel* servos[DXL_MAXSERVOS];

	int delay;

	DXLComm commPort;
	friend class Dynamixel;
public:
	DynamixelChain() :
		port(DEFAULT_DXL_PORT), numServos(0), delay(5000)
	{
		for (uint32_t i = 0; i < DXL_MAXSERVOS; ++i)
			servos[i] = NULL;
	}

	virtual ~DynamixelChain();
	virtual void initialize(ConfigElementPtr cfg);
	virtual Dynamixel* createServo(ConfigElementPtr cfg);

	inline unsigned int getNumServos() const {
		return numServos;
	}

	Dynamixel* getServo(int idx) {
		if (idx < 0 || idx >= numServos)
			return NULL;
		return servos[idx];
	}

	virtual inline bool readServos() {
		if (!commPort.connected()) {
//			usleep(1000);
			return false;
		}
		int tick;

		bool ok = true;
		for (unsigned int i = 0; i < numServos; i++) {
			ok &= servos[i]->readServo();
		}

		return ok;
	}

	virtual void connect() {
		// open port
		// Init DXL
		if (commPort.initialize(port.c_str(), 57600.0) != 1) {
			ERROR("Dynamixel: Could not open Dynamixel port\n");
			return;
		}

        // initialise servos
        for (unsigned int i = 0; i < numServos; i++) {
			if (servos[i] != NULL) {
				servos[i]->connect();
            }
        }

//		readServos();
//
//        // zero servos on start
//        for (unsigned int i = 0; i < numServos; i++) {
//        	Dynamixel2* servo = servos[i];
//			if (servo != NULL && servo->id >= 0 && servo->zeroOnStart) {
//				servos[i]->setPos(0);
//            }
//        }
	}

	virtual void disconnect() {
        for (unsigned int i = 0; i < numServos; i++) {
			if (servos[i] != NULL) {
				servos[i]->disconnect();
            }
        }

		// close port
		commPort.terminate();
	}
};

} // namespace casualty

#endif /* CASUALTY_DYNAMIXEL2_H_ */
