/*
 * arduino.cpp
 *
 *  Created on: 30/08/2012
 *      Author: timothyw
 */

#include <ros/ros.h>

#include <crosbot/data.hpp>

#include <negotiator_drivers/flippers/arduino.h>
#include <cstdio>
#include <cstdlib>
#include <cmath>

#define DEFAULT_PORT        "/dev/ttyACM0"

#ifndef DEG2RAD
#define DEG2RAD(A)        (M_PI*(A)/180.0)
#endif

// Driver codes
#define CMD_FORWARD        0x02
#define CMD_REV            0x03
#define CMD_EMSTOP         0x05

namespace casualty {

FlippersArduinoInterface::FlippersArduinoInterface() :
    //defaultSpeed(0x60),
    flipperOffset(0),
    debug(0)
{
    commsPort = NULL;
    threadRead = 0;
}

FlippersArduinoInterface::~FlippersArduinoInterface() {
}

void FlippersArduinoInterface::configure(crosbot::ConfigElementPtr cf) {
    // Load config
    port = cf->getParam("port", DEFAULT_PORT);
    flipperOffset = cf->getParamAsInt("zero", 0);
    flipperOffset = cf->getParamAsInt("offset", flipperOffset);
    debug = cf->getParamAsInt("debug", 0);

    // Create the serial communication port
    commsPort = NULL;
    commsPort = new Comms::SerialPort(port, B115200);
}

void FlippersArduinoInterface::start() {
    ROS_INFO("Starting NegotiatorFlippers driver");

    // Open the communication port
    ROS_INFO("Opening flippers on port %s", commsPort->getPort().c_str());
    if (!commsPort->openPort()) {
        ROS_ERROR("Unable to open port on %s", commsPort->getPort().c_str());
        exit(-1);
        return;
    }
}

void FlippersArduinoInterface::stop() {
    // Close the serial port
    commsPort->closePort();
}

// This code is from Adam Milstein
void FlippersArduinoInterface::readValues(double &pot_val, double &setPoint, double &battery) {
    unsigned char readBuff[10];

    /*
     * The communication protocol is:
     * %pot_val,setpoint$battery.
     */

    int read_size = 0;
    int ct;
    int localPot_val = 0, localSetPoint = 0, localBattery = 0;

    commsPort->flushInput();

    ct = 0;

    // Read off %
    do {
        read_size = commsPort->nReadData(readBuff, 1, 1, 1000);
    } while (read_size > 0 && readBuff[0] != '%');
    if (read_size <= 0) {
        ROS_ERROR("NegotiatorFlippers: Error reading start of data frame.");
        return;
    }
    ct = 0;

    // Read pot_val
    do {
        read_size = commsPort->nReadData(&(readBuff[ct]), 1, 1, 1000);
    } while (read_size > 0 && readBuff[ct++] != ',');
    if (read_size <= 0) {
        ROS_ERROR("NegotiatorFlippers: Error reading potentiometer value.");
        return;
    }
    readBuff[ct - 1] = 0;
    localPot_val = atoi((char *) readBuff);
    ct = 0;

    // Read set point
    do {
        read_size = commsPort->nReadData(&(readBuff[ct]), 1, 1, 1000);
    } while (read_size > 0 && readBuff[ct++] != '$');
    if (read_size <= 0) {
        ROS_ERROR("NegotiatorFlippers: Error reading set point.");
        return;
    }
    readBuff[ct - 1] = 0;
    localSetPoint = atoi((char *) readBuff);
    ct = 0;

    // Read battery
    do {
        read_size = commsPort->nReadData(&(readBuff[ct]), 1, 1, 1000);
    } while (read_size > 0 && readBuff[ct++] != '.');
    if (read_size <= 0) {
        ROS_ERROR("NegotiatorFlippers: Error reading battery voltage.");
        return;
    }
    readBuff[ct - 1] = 0;
    localBattery = atoi((char *) readBuff);

    //ROS_ERROR("%d, %d, %d", localPot_val, localSetPoint, localBattery);

    // Adjust for zero point/clutch alignment
    localPot_val += flipperOffset;
    if (localPot_val >= 360) {
        localPot_val = localPot_val - 360;
    } else if (localPot_val < 0) {
        localPot_val += 360;
    }

    // Correct voltage
    battery = battery / 10;

    // Set references
    pot_val = DEG2RAD(localPot_val);
    setPoint = localSetPoint;
    battery = localBattery;
}

void FlippersArduinoInterface::command_EMStop() {
    char data[2];
    data[0] = CMD_EMSTOP;
    data[1] = 0x00;
    commsPort->sendData(data, 2);
}

void FlippersArduinoInterface::command_Velocity(double velocity) {
    // Magnitude should be a byte from 0-127 (see polulu documentation)
    // Velocity is assumed to be a float from 0 to 1.
    char speedScale = 127;

    // Speeds from Arduino are reversed - fix this in arduino interface
    if (velocity < 0) {
        char command = speedScale*-velocity;
        if(debug) printf("forward command: %d\n", command);
        command_Forward(command);
    } else {
        char command = speedScale*velocity;
        if(debug) printf("reverse command: %d\n", command);
        command_Reverse(command);
    }
}

void FlippersArduinoInterface::command_Forward(char velocity) {
    char data[2];
    data[0] = CMD_FORWARD;
    data[1] = velocity;
    commsPort->sendData(data, 2);
}

void FlippersArduinoInterface::command_Reverse(char velocity) {
    char data[2];
    data[0] = CMD_REV;
    data[1] = velocity;
    commsPort->sendData(data, 2);
}

} // namespace casualty
