/*
 * emu_arm.hpp
 *
 *  Created on: 05/06/2013
 *      Author: mmcgill
 */

#ifndef CASUALTY_EMU_ARM_HPP_
#define CASUALTY_EMU_ARM_HPP_

#include <polulu_jrk/polulu_jrk.hpp>
#include <dynamixel/dynamixel.h>

namespace casualty {
using namespace crosbot;

class EmuArm {
protected:
	// dynamixels for neck
	DynamixelChain neck;
	Dynamixel *neckTilt, *neckPan;

	// Polulus for arm
	PololuArrayJRK elbow, shoulder, base;

	class DXLThread : public Thread {
	public:
		EmuArm& arm;
		bool operating;
		DXLThread(EmuArm& arm) : arm(arm), operating(true) {}
		void run() {
			arm.dxlThreadBody();
		}
	};
	DXLThread dxlThread;

public:

	EmuArm() : neckTilt(NULL), neckPan(NULL), dxlThread(*this) {}
	virtual ~EmuArm() {}

	virtual void Initialize(crosbot::ConfigElementPtr cfg);
	void dxlThreadBody();

	virtual void Shutdown() {
		base.Shutdown();
		shoulder.Shutdown();
		elbow.Shutdown();
		dxlThread.operating = false;
		neck.disconnect();
	}

	virtual void updatePoses() {}

	// Look at a point relative to robot.
	bool lookAtObject(Point p, const std::string& frame);
	virtual bool getNeckAngles(Point point, const std::string& frame, double& tilt, double& pan) {return false;}
};


} // namespace casualty



#endif /* CASUALTY_EMU_ARM_HPP_ */
