/*
 * flippers.cpp
 *
 *  Created on: 19 Aug 2015
 *      Author: timothyw
 */

#include <ros/ros.h>
#include "negotiator_drivers/flippers/flippers.h"

#include <algorithm>

#define DEFAULT_JOINTNAME    "flipper"

#define ROUNDING_ERROR        0.05

namespace casualty {

inline double angleDiff(double a1, double a2) {
    double diff = a1 - a2;
    NORMALISE_ANGLE(diff);
    return diff;
}

FlippersROS::FlippersROS(FlippersHardwareInterface *interface) :
    angleAccept(DEG2RAD(5)),
    seqNum(0), lastCommand(ros::Time::now()), maxCommandDelay(1),
    battery(0), setPoint(0), potentiometer(0),
    operating(false),
    desiredPos(INFINITY), velocity(0), stopped(false),
    jointName(DEFAULT_JOINTNAME),
    interface(interface),
    threadRead(0)
{
    ROS_INFO("FlippersROS: initialising");
    if (interface == NULL) {
        ROS_ERROR("FlippersROS :: No Hardware interface provided");
    }
}

void FlippersROS::configure(crosbot::ConfigElementPtr cf) {
    // ROS Node configuration
    ROS_INFO("FlippersROS: configure");
    jointName = cf->getParam("joint", DEFAULT_JOINTNAME);
    operating = false;

    // Configure hardware interface
    interface->configure(cf);
}

void* _FlippersROS_threadRun(void *threadPtr) {
    FlippersROS* flippers = (FlippersROS*) threadPtr;
    flippers->run();
    return NULL;
}

void FlippersROS::connect() {
    // Start hardware communication, connect to port, etc...
    ROS_INFO("FlippersROS :: connect()");
    interface->start();

    // Ensure thread has started
    usleep(1000000);

    ros::NodeHandle nh;
    jointPub = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
    jointSub = nh.subscribe("joint_control", 1, &FlippersROS::callback_setJoint, this);

    // Set operating
    operating = true;

    // Start reading thread
//    ROS_INFO("Starting read thread\n");
    int err = pthread_create(&threadRead, NULL, _FlippersROS_threadRun, this);
    if (err != 0) {
        ROS_ERROR("FlippersROS :: Could not start reading thread. Error: %d", err);
    }
}

void FlippersROS::disconnect() {
    // Terminate thread
    ROS_INFO("FlippersROS :: Disconnect()");
    operating = false;

    // Stop motors
    interface->command_EMStop();

    // Disconnect hardware interface
    interface->stop();
}

// Thread loop
void FlippersROS::run() {
    double pot_val;
    double setPoint;
    double battery;

//    ROS_INFO("Reading thread started\n");
    while (operating) {
        // Read values and store
        interface->readValues(pot_val, setPoint, battery);
        potentiometer = pot_val;
        this->setPoint = setPoint;
        this->battery = battery;

        // Process set point movement
        setJointSetPoint();

        // Call internal set
        publishValues();
    }
//    ROS_INFO("Reading thread stopped\n");
}

void FlippersROS::callback_setJoint(sensor_msgs::JointStateConstPtr jstate) {
    double setPoint = INFINITY;
    double velocity = 0;
    double oldVel = this->velocity;

    bool jointCommand = false;
    bool setPointCommand = false;
    bool velocityCommand = false;

    // Load command from ROS message
    for (unsigned int i = 0; i < jstate->name.size(); ++i) {
        if (jstate->name[i] == jointName) {
            jointCommand = true;
            if (jstate->position.size() > i) {
                setPoint = jstate->position[i];
                setPointCommand = true;
            }
            if (jstate->velocity.size() > i) {
                velocity = jstate->velocity[i];
                velocityCommand = true;
            }
        }
    }

    // If command received for the flippers
    if (jointCommand) {
        lastCommand = ros::Time::now();

        if (setPointCommand) {
            // For a set point command, configure velocity command to stop if necessary
            this->desiredPos = setPoint;
            NORMALISE_ANGLE(this->desiredPos);

            if (fabs(angleDiff(setPoint, potentiometer)) < angleAccept) {
                // At desired angle so stop.
                velocity = 0;
            }
        } else {
            this->desiredPos = INFINITY;
        }

        // Send command via interface based on velocity
        if (fabs(velocity) < ROUNDING_ERROR) {
            stopped = true;
            interface->command_EMStop();
            velocity = 0;
        } else {
            if (velocity != oldVel) {
                // Scale velocity between -1 to 1
                velocity = std::max(-1.0, velocity);
                velocity = std::min(1.0, velocity);
                //ROS_INFO("FlippersROS: set velocity: %.2lf", velocity);
                interface->command_Velocity(velocity);
            }

            stopped = false;
        }

        // Update stored values
        this->velocity = velocity;
    }

}

void FlippersROS::publishValues() {
    if (!jointPub) {
        return;
    }

    // Publish joint state
    sensor_msgs::JointState state;
    state.header.frame_id = "base_link";
    state.header.stamp = ros::Time::now();
    state.header.seq = seqNum++;

    state.name.push_back(jointName);
    state.position.push_back(potentiometer);
    state.name.push_back("flipper_left_joint");
    state.position.push_back(potentiometer);
    state.name.push_back("flipper_right_joint");
    state.position.push_back(potentiometer);
    state.velocity.push_back(velocity);
    state.effort.push_back(NAN);

    jointPub.publish(state);
}

void FlippersROS::setJointSetPoint() {
    if ((ros::Time::now() - lastCommand).toSec() > maxCommandDelay ||
            fabs(angleDiff(desiredPos, potentiometer)) < angleAccept) {
        // Command timeout / Flipper at location
        if (!stopped) {
            stopped = true;
            velocity = 0;
            interface->command_EMStop();
        }
    }
}

} // namespace casualty

