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

#include <ros/ros.h>
#include "negotiator_drivers/state/monitor.h"

#include <crosbot/geometry.hpp>
#include <crosbot/utils.hpp>
#include <geometry_msgs/PoseStamped.h>

#include <cstdio>
#include <cstdlib>

#define SEC2USEC(n) (n * 1000000)

#define DEFAULT_FLIPPER_READINGS    1
#define DEFAULT_FRAME_ROBOT         "/base_footprint"
#define DEFAULT_FRAME_ICP           "/icp_test"
#define DEFAULT_FRAME_SLAM          "/slam"
#define DEFAULT_FROM_GAZEBO            false
#define DEFAULT_MAXWAIT_TRANSFORM   2.0
#define DEFAULT_PUBLISH_SLEEP       0.1
#define DEFAULT_PUBLISHER_ENABLED   false
#define DEFAULT_SLAM_ENABLED        false
#define ERROR_FREQUENCY             5.0
#define LOG_START                   "NegotiatorMonitor ::"

namespace casualty {

NegotiatorMonitor::NegotiatorMonitor() {
    flipperSum = 0;
    fromGazebo = DEFAULT_FROM_GAZEBO;
    frequency = DEFAULT_PUBLISH_SLEEP;
    nFlipperReadings = DEFAULT_FLIPPER_READINGS;
    operating = false;
    publisherEnabled = false;
    publishSleep = SEC2USEC(frequency);
    slamEnabled = DEFAULT_SLAM_ENABLED;
    tfListener = NULL;
    threadPublish = 0;
}

NegotiatorMonitor::~NegotiatorMonitor() {
    operating = false;
}

void NegotiatorMonitor::configure(ros::NodeHandle& nh) {
    nh.param<double>("frequency", frequency, DEFAULT_PUBLISH_SLEEP);
    publishSleep = SEC2USEC(frequency);

    nh.param<std::string>("frameRobot", frameRobot, DEFAULT_FRAME_ROBOT);
    nh.param<std::string>("frameIcp", frameIcp, DEFAULT_FRAME_ICP);
    nh.param<std::string>("frameSlam", frameSlam, DEFAULT_FRAME_SLAM);
    nh.param<bool>("gazebo", fromGazebo, DEFAULT_FROM_GAZEBO);
    nh.param<int>("flipperReadings", nFlipperReadings, DEFAULT_FLIPPER_READINGS);
    nh.param<bool>("publisherEnabled", publisherEnabled, DEFAULT_PUBLISHER_ENABLED);
    nh.param<bool>("slamEnabled", slamEnabled, DEFAULT_SLAM_ENABLED);
}

void* _NegotiatorMonitor_startPublisher(void *threadPtr) {
    NegotiatorMonitor* monitor = (NegotiatorMonitor*) threadPtr;
    monitor->publish();
    return NULL;
}

void NegotiatorMonitor::connect() {
    LOG("%s connect()\n", LOG_START);

    // Initialise Errors
    lastICPError = ros::Time(0);

    // Initialise state
    // Init time to 0 for rosbag play
    ros::Time initTime = ros::Time(0);
    timeFlippers = initTime;
    state.flippers = 0;
    flipperReadings.clear();
    for (int i = 0; i != nFlipperReadings; ++i) {
        flipperReadings.push_back(0);
    }
    flipperSum = 0;

    // Subscribe
    ros::NodeHandle nh;
    if (fromGazebo) {
        subFlippers = nh.subscribe("flippers", 1, &NegotiatorMonitor::callbackFlipperGazebo, this);
        subXsense = nh.subscribe("xsens", 1, &NegotiatorMonitor::callbackXsensGazebo, this);
    } else {
        subFlippers = nh.subscribe("flippers", 1, &NegotiatorMonitor::callbackFlipper, this);
        subXsense = nh.subscribe("xsens", 1, &NegotiatorMonitor::callbackXsens, this);
    }
    subTracks = nh.subscribe("tracks", 1, &NegotiatorMonitor::callbackTracks, this);
    tfListener = new tf::TransformListener(ros::Duration(30));

    // Advertise
    serviceState = nh.advertiseService("state_srv", &NegotiatorMonitor::callbackStateService, this);
    pubState = nh.advertise<negotiator_drivers::NegotiatorStateMsg>("state_pub", 1);
    //LOG("%sService Advertised\n", LOG_START);

    // Start monitor publisher thread
    if (publisherEnabled) {
        operating = true;
        //LOG("Starting read thread\n");
        int err = pthread_create(&threadPublish, NULL, _NegotiatorMonitor_startPublisher, this);
        if (err != 0) {
            ROS_ERROR("Could not start reading thread. Error: %d", err);
        }
    }
}

void NegotiatorMonitor::disconnect() {
    printf("%s disconnect()\n", LOG_START);

    // Stop monitor publisher thread
    if (publisherEnabled) {
        operating = false;
        int err = pthread_join(threadPublish, NULL);
        if (err != 0) {
            ROS_ERROR("Error joining terminating Publish thread. Error: %d", err);
        }
    }

    // Cleanup
    state.flippers = 0;
}

NegotiatorStatePtr NegotiatorMonitor::getState() {
    NegotiatorStatePtr state = new NegotiatorState();

    // Lock every mutex
    crosbot::Lock lockFlippers(mutexFlippers);
    crosbot::Lock lockTracks(mutexTracks);
    crosbot::Lock lockXsens(mutexXsens);

    // Lookup time
    // TODO: Remove tZero hack for corrupted bag files - REMOVE
    ros::Time time = ros::Time::now();
    ros::Time tZero = ros::Time(0);
    //LOG("%stime=%d.%d\n", LOG_START, time.sec, time.nsec);

    // Transform the pose
    crosbot::Pose3D toPoseIcp;
    crosbot::Pose3D toPoseSlam;
    try {
        // Lookup transform
        tf::StampedTransform transform;
        tfListener->lookupTransform(frameIcp, frameRobot, tZero, transform);

        // Create ICP Pose
        toPoseIcp = crosbot::Pose3D(transform);
    } catch (tf::TransformException& e) {
        if (time - lastICPError > ros::Duration(ERROR_FREQUENCY)) {
            ROS_ERROR("%s Unable to lookup current pose: %s", LOG_START, e.what());
            lastICPError = time;
        }
    }
    if (slamEnabled) {
        try {
            // Lookup transform
            tf::StampedTransform transform;
            tfListener->lookupTransform(frameSlam, frameRobot, tZero, transform);

            // Create ICP Pose
            toPoseSlam = crosbot::Pose3D(transform);
        } catch (tf::TransformException& e) {
            if (time - lastSlamError > ros::Duration(ERROR_FREQUENCY)) {
                ROS_ERROR("%s Unable to lookup current pose: %s", LOG_START, e.what());
                lastSlamError = time;
            }
        }
    }

    state->time = time;
    state->poseIcp = toPoseIcp;
    state->poseSlam = toPoseSlam;
    state->flippers = this->state.flippers;
    state->xsens = this->state.xsens;
    state->trackLinear = this->state.trackLinear;
    state->trackAngular = this->state.trackAngular;

    return state;
}

void NegotiatorMonitor::callbackFlipper(sensor_msgs::JointStateConstPtr jstate) {
    crosbot::Lock lock(mutexFlippers);

    if (jstate->header.stamp > timeFlippers) {
        for (int i = 0; i != jstate->name.size(); ++i) {
            if (jstate->name[i] == "flipper") {
                // Update flipper value w/ smoothing
                double oldValue = flipperReadings.front();
                double newValue = jstate->position[i];
                flipperReadings.pop_front();
                flipperReadings.push_back(newValue);
                flipperSum = flipperSum - oldValue + newValue;

                state.flippers = flipperSum / nFlipperReadings;
                timeFlippers = jstate->header.stamp;
                //ROS_INFO("%s reading: %.1lf - smoothed: %.1lf", LOG_START, newValue, state.flippers);
            }
        }
    }
}

void NegotiatorMonitor::callbackFlipperGazebo(std_msgs::Float64 position) {
    crosbot::Lock lock(mutexFlippers);
    state.flippers = position.data;
    timeFlippers = ros::Time::now();
}

/*void NegotiatorMonitor::callbackTracks(nav_msgs::OdometryConstPtr tracks) {
    crosbot::Lock lock(mutexTracks);

    state.trackLinear = tracks->twist.twist.linear.x;
    state.trackAngular = tracks->twist.twist.angular.z;
}*/

void NegotiatorMonitor::callbackTracks(geometry_msgs::TwistPtr tracks) {
    crosbot::Lock lock(mutexTracks);
    state.trackLinear = tracks->linear.x;
    state.trackAngular = tracks->angular.z;
}

void NegotiatorMonitor::callbackXsens(geometry_msgs::QuaternionConstPtr xsens) {
    crosbot::Lock lock(mutexXsens);

    state.xsens = *xsens;
}

void NegotiatorMonitor::callbackXsensGazebo(sensor_msgs::ImuPtr xsens) {
    crosbot::Lock lock(mutexXsens);

    state.xsens = xsens->orientation;
}

bool NegotiatorMonitor::callbackStateService(
        negotiator_drivers::NegotiatorStateServiceRequest& request,
        negotiator_drivers::NegotiatorStateServiceResponse& response) {
    NegotiatorStatePtr state = getState();

    response.state = state->toROS();
    response.state.header.stamp = state->time;

    return true;
}

void NegotiatorMonitor::publish() {
    while (operating) {
        //LOG("%s Publish Cycle\n", LOG_START);

        NegotiatorStatePtr state = getState();
        negotiator_drivers::NegotiatorStateMsg msg = state->toROS();
        pubState.publish(msg);

        usleep(publishSleep);
    }
}

} // namespace casualty
