/*
 * autolevel.h
 *
 *  Created on: 04/09/2012
 *      Author: mmcgill
 */

#ifndef CASUALTY_AUTOLEVEL_H_
#define CASUALTY_AUTOLEVEL_H_

#include <crosbot/handle.hpp>
#include <crosbot/config.hpp>
#include <crosbot/thread.hpp>
#include <xsens/xsens.h>

#include <autolevel/autolevelChain.hpp>
#include <autolevel/factories.hpp>

#ifndef MIN
#define MIN(X, Y)		(((X)<(Y))?(X):(Y))
#endif

namespace casualty {
using namespace crosbot;

class AutoLevel : public crosbot::HandledObject {
public:
	AutoLevel();
	AutoLevel(FactoryXSensPtr xsensFactory, FactoryAutoLevelChainPtr autolevelChainFactory);
	~AutoLevel();

	void configure(ConfigElementPtr config);
	void start();
	void stop();

	virtual void updateOrientation(const Quaternion& orientation) {}
	virtual void updateJoints(double pitch, double roll, double scan) {}

	class MTiThread : public Thread {
		AutoLevel& level;
	public:
		MTiThread(AutoLevel& level) : level(level) {}

		void run() {
			level.mtiThreadBody();
		}
	};
	class DxlThread : public Thread {
		AutoLevel& level;
	public:
		DxlThread(AutoLevel& level) : level(level) {}

		void run() {
			level.dxlThreadBody();
		}
	};

	// State is broadcast as a topic, therefore make enum public
	enum StowState { Unknown, Ready, Stowing, Unstowing, Parking };
protected:

	// Auto level operation
	bool operating;

	// Factories for creating xsens and dynamixel chains
	FactoryXSensPtr xsensFactory;
	FactoryAutoLevelChainPtr autolevelChainFactory;

	// XSens state
	StowState state;
	XSensPtr xsens;

	// Dynamixel chain
	AutoLevelChainPtr dxlChain;
	double stowAngle, unstowAngle, stowErr;
	AutoLevelServo *tiltServo, *rollServo, *scanServo;

//	std::vector< std::vector< double > > stowPoints;
//	int nextStowPoint;
//	std::vector< double >* parkLocation;

	MTiThread mtiThread;
	DxlThread dxlThread;

	void mtiThreadBody();
	void dxlThreadBody();
	friend class MTiThread;
	friend class DxlThread;

	void updateServos(Quaternion orientation);
	void stow();
	void unstow();
	void park();
//	int getNearestStowPoint();

	inline void gotoStowPoint(std::vector< double >& stowPoint) {
		unsigned int minI = MIN(stowPoint.size(), dxlChain->getNumServos());
		for (unsigned int i = 0; i < minI; i++) {
			if (stowPoint[i] != INFINITY) {
//			    LOG("gotoStowPoint - 1\n");
				dxlChain->getServo(i)->setPos(stowPoint[i]);
//                LOG("gotoStowPoint - 2\n");
			}
		}
	}

	inline bool atStowPoint(std::vector< double >& stowPoint) {
		unsigned int minI = MIN(stowPoint.size(), dxlChain->getNumServos());

		for (unsigned int i = 0; i < minI; i++) {
		    double d = stowPoint[i] - dxlChain->getServo(i)->getPos();
			if (fabs(d) > stowErr) {
				return false;
			}
		}
		return true;
	}
};
typedef Handle<AutoLevel> AutoLevelPtr;

} // namespace casualty


#endif /* CASUALTY_AUTOLEVEL_H_ */
