/*
 * NegotiatorState.h
 *
 *  Created on: 08/04/2014
 *      Author: timothyw
 */

#ifndef NEGOTIATOR_STATE_H_
#define NEGOTIATOR_STATE_H_

#include <crosbot/handle.hpp>
#include <crosbot/geometry.hpp>
#include "negotiator_drivers/NegotiatorStateMsg.h"

namespace casualty {

class NegotiatorState : public crosbot::HandledObject {
public:
    NegotiatorState() :
        time(),
        poseIcp(),
        trackLinear(0),
        trackAngular(0),
        xsens(),
        flippers(0)
    {};

    // Timestamp for this state
    ros::Time time;

    // Pose (position & orientation)
    crosbot::Pose poseIcp;
    crosbot::Pose poseSlam;

    // Track velocities
    double trackLinear;
    double trackAngular;

    // Xsens
    crosbot::Quaternion xsens;

    // Flippers
    double flippers;

    // Arm?

#ifdef ROS_VERSION
    NegotiatorState(const negotiator_drivers::NegotiatorStateMsg& m) :
        time(m.header.stamp),
        poseIcp(crosbot::Pose3D(m.pose)),
        trackLinear(m.trackLinear),
        trackAngular(m.trackAngular),
        xsens(m.xsens),
        flippers(m.flippers)
    {}

    inline negotiator_drivers::NegotiatorStateMsg toROS() const {
        negotiator_drivers::NegotiatorStateMsg msg;
        msg.header.stamp = time;
        msg.pose = poseIcp.toROS();
        msg.trackLinear = trackLinear;
        msg.trackAngular = trackAngular;
        msg.xsens = xsens.toROS();
        msg.flippers = flippers;
        return msg;
    }

#endif
};
typedef crosbot::Handle<NegotiatorState> NegotiatorStatePtr;

} // namespace casualty

#endif /* NEGOTIATOR_STATE_H_ */
