/*
 * polulu_jrk.hpp
 *
 *  Created on: 02/06/2013
 *      Author: mmcgill
 */

#ifndef CASUALTY_POLULU_JRK_HPP_
#define CASUALTY_POLULU_JRK_HPP_

#include <exception>
#include <string>
#include <vector>

#include <unistd.h>

#include <crosbot/config.hpp>
#include <crosbot/thread.hpp>
#include <crosbot/utils.hpp>

namespace casualty {

#ifndef MAX
	#define MAX(X, Y)	((X)>(Y)?(X):(Y))
#endif

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

class PoluluException : public std::exception {};

////////////////////////////////////////////////////////////////////////////////
// The class for the driver
class PololuArrayJRK {
public:

	// Constructor; need that
	PololuArrayJRK();
	virtual ~PololuArrayJRK();

	void Initialize(crosbot::ConfigElementPtr cf);
	void Shutdown() { StopThread(); }

	// Must implement the following methods.
//	int MainSetup();
//	void MainQuit();

	// TODO: remove these when finished
//	struct QueuePointer {};
//	struct player_msghdr {};
	void StartThread(){ if (!listener.isAlive()) listener.start(); }
	void StopThread() {listener.operating = false; usleep(100000); }

	// This method will be invoked on each incoming message
//	int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);

	struct ServoSettings {
		int id;

		// These parameters are applied in this order.
		// 1: Input is bounds checked (min/max)
		// 2: The result is shifted by the offset
		// 3: The result is reversed if necessary
		float minRad;
		float maxRad;
		int tickZero;
		float radPerTick;
		int reverse;
		int brakeAtDestination;
		int brakedAtDestination;
		int zeroOnStartup;


		int mode;
		unsigned short lastCommandedTicks;
		unsigned short currentAngleTicks;

		ServoSettings() : id(-1),
			minRad(DEG2RAD(-150)), maxRad(DEG2RAD(150)),
			tickZero(512), reverse(0), radPerTick(DEG2RAD(0.09)),
			brakeAtDestination(0), brakedAtDestination(0),
			zeroOnStartup(0),
			lastCommandedTicks(0xFFFF), currentAngleTicks(0xFFFF), mode(0)
		{}

		inline unsigned short Rad2Tick(float rad) {
			rad = MAX(rad, minRad);
			rad = MIN(rad, maxRad);

			unsigned short rVal =
					(unsigned short)((reverse?-1:1)*(rad)/radPerTick) + tickZero;
		}

		inline float TickToRad(unsigned short tick) {
			float rVal = (reverse?-1:1)*(((int)tick - tickZero)*radPerTick);
			return rVal;
		}

		inline float GetCurrentRad() {
			return TickToRad(currentAngleTicks);
		}
	};
protected:

	// Main function for device thread.
//	void Main();
//	void FromPololuToRad(unsigned short inPololu, float * outRad, int index);
//	void FromRadToPololu(float inRad, unsigned short * outPololu, int index);
	int OpenPort();
	int ClosePort();
	void motor_send(int index, unsigned short position, int brake);
	void motor_send_vel(int index, float vel, int brake);
	int ReadPosition(ServoSettings& servo, unsigned short * outTicks);
	virtual void PublishPosition() { if(debug) LOG("Current reading: %d, %.2frad\n", servos[0].currentAngleTicks, servos[0].TickToRad(servos[0].currentAngleTicks)); }

	std::string port_name;
	int fd;

	void listenThread();
	class PoluluThread : public crosbot::Thread {
	public:
		PololuArrayJRK& array;
		bool operating;

		PoluluThread(PololuArrayJRK& array) : array(array), operating(true) {}
		void run() {
			array.listenThread();
		}
	};
	PoluluThread listener;
	int numServos;
	std::vector< ServoSettings > servos;

	int debug, search;
public:

	float GetPosition(int index=0) {
		ServoSettings& servo = servos[index];
		return servo.GetCurrentRad();
	}

	void SetPosition(float pos, int index=0);
	void SetVelocity(float vel, int index=0);
};//}}}


} // namespace casualty

#endif /* CASUALTY_POLULU_JRK_HPP_ */
