/*
 * autolevelChain.hpp
 *
 * AutoLevel Chain - extensions of Dynamixel Chain
 *
 *  Created on: 20 Oct 2014
 *      Author: timothyw
 */

#ifndef CASUALTY_AUTOLEVEL_CHAIN_H_
#define CASUALTY_AUTOLEVEL_CHAIN_H_

#include <crosbot/config.hpp>

#include <dynamixel/dynamixel.h>

namespace casualty {
using namespace crosbot;

/**
 * Chain of dynamixels for the autoleveler
 */
class AutoLevelChain : public DynamixelChain, public crosbot::HandledObject {
public:
	virtual Dynamixel* createServo(ConfigElementPtr cfg);

protected:
	// Make the Servo Object
	virtual Dynamixel* make(ConfigElementPtr cfg);

	// Configure the sero object and add to the chain
	virtual void configure(Dynamixel* rval, ConfigElementPtr cfg);
};
typedef crosbot::Handle<AutoLevelChain> AutoLevelChainPtr;

/**
 * Individual servo, which needs to know it's location in the chain
 */
class AutoLevelServo : public Dynamixel {
public:
	typedef enum { Servo, AutoLevelRoll, AutoLevelTilt, AutoLevelScan, Constant } DXLType;

	AutoLevelServo(AutoLevelChain& chain) :
		Dynamixel(chain), type(Servo), stow(INFINITY), park(INFINITY), constant(0), offset(0)
	{}
	virtual ~AutoLevelServo() {};

	virtual void initialize(crosbot::ConfigElementPtr cfg);
	virtual void adjustLevel(Quaternion orient);

	double getConstant() const { return constant; }
	double getPark() const { return park; }
	double getStow() const { return stow; }
	DXLType getType() const { return type; }

protected:
	DXLType type;
	double stow, park, constant;
	double offset;
};

} // namespace casualty

#endif /* CASUALTY_AUTOLEVEL_CHAIN_H_ */
