/*
 * File         : victimpanel.h
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 09/02/2010
 * Description  : Victim Id Panel - based on existing SnapPanel
 * Copyright    :
 */

#include "victim_id/gui/victimpanel.hpp"

#include "victim_id/states/cameraInspect.hpp"
#include "victim_id/states/centreBlob.hpp"
#include "victim_id/states/driveToBlob.hpp"
#include "victim_id/states/explore.hpp"
#include "victim_id/states/initial.hpp"
#include "victim_id/states/investigate.hpp"
#include "victim_id/states/lostHotSpot.hpp"
#include "victim_id/states/paused.hpp"
#include "victim_id/states/victim.hpp"
#include <std_msgs/String.h>

#include <typeinfo>
#include <QtGui/QHBoxLayout>
#include <QtGui/QLabel>
#include <QtGui/QPainter>
//#include <QtGui/QUrl>
#include <QtGui/QHBoxLayout>
#include <QtGui/QVBoxLayout>
#include <utility>

#define ATTRIB_AUTOVICTIMID     "autovictimid"
#define LOG_START_P             "VictimIdViewPanel :: "
#define LOG_START_W             "VictimIdViewWidget :: "
#define REFRESH_THREAD          "RefreshThread"
#define STATUS_DIAMETER         10

// Colours
#define COLOUR_NOTUSED          QColor(255, 255, 0)
#define COLOUR_NOVIC            QColor(0, 255, 0)
#define COLOUR_VICTIM           QColor(255, 0, 0)

// Status numbers
#define STATUS_NO               8
#define STATUS_RUNNING          0
#define STATUS_STATE            1
#define STATUS_THRESHOLD        2
#define STATUS_BLOBCOUNT        3
#define STATUS_DISTANCE         4
#define STATUS_BLOBCENT         5
#define STATUS_BLOBPOS          6
#define STATUS_WALLMODE         7
// Status' that are combined with others
#define STATUS_VICTIM           255
#define STATUS_RUN_DT           254

using namespace crosbot;
using namespace crosbot::gui;
using namespace casualty;
using namespace casualty::gui;
using std::map;
using std::string;
using std::vector;

namespace casualty {
namespace gui {

VictimIdViewWidget::VictimIdViewWidget() :
        QWidget(),
        autoVictimGB("Automatic Victim Detection"),
        victimLB("")
{
    // Setup feedback components - status labels & circles
    QVBoxLayout *autoVicLayout = new QVBoxLayout();
    for (int i = 0; i < STATUS_NO; ++i) {
        if (i == STATUS_VICTIM) continue;

        QHBoxLayout *horz = new QHBoxLayout();
        QStatusCircle *sCircle = new QStatusCircle(COLOUR_NOVIC);
        QLabel *sLabel = new QLabel("");
        sLabel->setAlignment(Qt::AlignTop);
        sLabel->setAlignment(Qt::AlignLeft);
        horz->addWidget(sCircle, 1);
        horz->addWidget(sLabel, 9);
        autoVicLayout->addLayout(horz);
        statusLabels.push_back(sLabel);
        statusCircles.push_back(sCircle);
    }

    // Set up label
    victimLB.setAutoFillBackground(true);
    victimLB.setAlignment(Qt::AlignCenter);
    autoVictimGB.setLayout(autoVicLayout);
    autoVicLayout->addWidget(&victimLB, 10);

    // Setup buttons
    pauseBtn.setText("Pause");
    pauseBtn.setEnabled(true);
    onlyDtBtn.setText("Toggle  DT");
    onlyDtBtn.setEnabled(true);
    leftWallBtn.setText("Left Wall");
    leftWallBtn.setEnabled(true);
    rightWallBtn.setText("Right Wall");
    rightWallBtn.setEnabled(true);
    QHBoxLayout *buttonLayout1 = new QHBoxLayout();
    buttonLayout1->addWidget(&pauseBtn, 1);
    buttonLayout1->addWidget(&onlyDtBtn, 1);
    autoVicLayout->addLayout(buttonLayout1, 1);
    QHBoxLayout *buttonLayout2 = new QHBoxLayout();
    buttonLayout2->addWidget(&leftWallBtn, 1);
    buttonLayout2->addWidget(&rightWallBtn, 1);
    autoVicLayout->addLayout(buttonLayout2, 1);

    QVBoxLayout *vlayout = new QVBoxLayout();
    vlayout->addWidget(&autoVictimGB, 1);
    setLayout(vlayout);

    // Load actypesStrings Map
    stateDescStrings[VID_STATE_CAMERA_INSPECT]     = "Camera Inspect";
    stateDescStrings[VID_STATE_CENTRE_BLOB]        = "Centre Blob";
    stateDescStrings[VID_STATE_DRIVE_TO_BLOB]      = "Drive to Blob";
    stateDescStrings[VID_STATE_EXPLORE]            = "Explore";
    stateDescStrings[VID_STATE_INTIAL]             = "Initial";
    stateDescStrings[VID_STATE_INVESTIGATE]        = "Investigate";
    stateDescStrings[VID_STATE_LOST_HOT_STOP]      = "Lost Hot Spot";
    stateDescStrings[VID_STATE_NO_STATE]           = "No state";
    stateDescStrings[VID_STATE_PAUSED]             = "Paused";
    stateDescStrings[VID_STATE_VICTIM]             = "Victim!!";
    paused = true;

    // Connect signals
    connect(this, SIGNAL(updateNeeded()), this, SLOT(updateFeedback()));
    connect(&pauseBtn, SIGNAL(clicked()), this, SLOT(callbackPausedBtn()));
    connect(&onlyDtBtn, SIGNAL(clicked()), this, SLOT(callbackOnlyDtBtn()));
    connect(&leftWallBtn, SIGNAL(clicked()), this, SLOT(callbackLeftWallBtn()));
    connect(&rightWallBtn, SIGNAL(clicked()), this, SLOT(callbackRightWallBtn()));

    // Setup ROS subscriber callback
    ros::NodeHandle nh;
    vicDataSub = nh.subscribe("vic_data_in", 1, &VictimIdViewWidget::callbackVicData, this);
    vicControlPub = nh.advertise<std_msgs::String>("vic_control", 1);

    LOG("%s Loaded\n", LOG_START_W);
}

VictimIdViewWidget::~VictimIdViewWidget() {
}

void VictimIdViewWidget::configure(ConfigElementPtr config) {
}

void VictimIdViewWidget::start() {
    LOG("%sStarting.\n", LOG_START_W);

    Q_EMIT updateNeeded();
}

void VictimIdViewWidget::stop() {
    data.clear();
}

void VictimIdViewWidget::callbackVicData(const victim_id::VictimIdDataMsgPtr msg) {
    // Load data
    char buffer[1000];
    //LOG("%s callbackVicData\n", LOG_START_W);
    paused = msg->paused;
    data[STATUS_RUNNING] = (msg->paused) ? "Paused" : "Running";
    if (msg->onlyRunDT) data[STATUS_RUNNING] = data[STATUS_RUNNING] + " (only DT)";
    data[STATUS_STATE] = stateDescStrings[msg->state];
    data[STATUS_VICTIM] = (msg->state == VID_STATE_VICTIM) ? "Victim Found" : "No Victim";
    sprintf(buffer, "%d", msg->blobsThreshold);
    data[STATUS_THRESHOLD] = string(buffer);
    sprintf(buffer, "%d", msg->blobsCount);
    data[STATUS_BLOBCOUNT] = string(buffer);
    sprintf(buffer, "%d", msg->blobDistance);
    data[STATUS_DISTANCE] = string(buffer);
    data[STATUS_BLOBCENT] = "";
    data[STATUS_BLOBPOS] = Point3D(msg->blobPosition).toString();
    data[STATUS_WALLMODE] = msg->wallFollowMode;

    // Set colours
    colour[STATUS_RUNNING] = (msg->paused) ? COLOUR_VICTIM : COLOUR_NOVIC;
    colour[STATUS_STATE] = (msg->state == VID_STATE_VICTIM) ? COLOUR_VICTIM : COLOUR_NOVIC;
    colour[STATUS_VICTIM] = (msg->state == VID_STATE_VICTIM) ? COLOUR_VICTIM : COLOUR_NOTUSED;
    colour[STATUS_THRESHOLD] = COLOUR_NOTUSED;
    colour[STATUS_BLOBCOUNT] = COLOUR_NOTUSED;
    colour[STATUS_DISTANCE] = COLOUR_NOTUSED;
    colour[STATUS_BLOBCENT] = COLOUR_NOTUSED;
    colour[STATUS_BLOBPOS] = COLOUR_NOTUSED;
    colour[STATUS_WALLMODE] = COLOUR_NOTUSED;

    // Update gui
    Q_EMIT updateNeeded();
}

void VictimIdViewWidget::updateFeedback() {
    // Get data
    generateFeedback(STATUS_RUNNING, "Executing");
    generateFeedback(STATUS_STATE, "Autonomy State");
    generateFeedback(STATUS_BLOBCOUNT, "Blob Count");
    generateFeedback(STATUS_DISTANCE, "Distance");
    generateFeedback(STATUS_THRESHOLD, "Threshold");
    generateFeedback(STATUS_BLOBCENT, "Blob Cent");
    generateFeedback(STATUS_BLOBPOS, "Blob Pos");
    generateFeedback(STATUS_WALLMODE, "Wall Follow");

    // Update Victim Status
    //LOG("%sVICTIM: %s\n", LOG_START_RT, result.c_str());
    QPalette vicPalette = victimLB.palette();
    QColor victimColour = colour[STATUS_VICTIM];
    string vicLabel = data[STATUS_VICTIM];
    vicPalette.setColor(QPalette::Window, victimColour);
    victimLB.setPalette(vicPalette);
    victimLB.setText(QString(vicLabel.c_str()));

    // Set buttons
    pauseBtn.setEnabled(true);
    if (paused) {
        pauseBtn.setText("Resume");
    } else {
        pauseBtn.setText("Pause");
    }

    // Do update
    //LOG("%s DO update\n", LOG_START_W);
    autoVictimGB.update();
}

void VictimIdViewWidget::callbackPausedBtn() {
    //LOG("%s callbackPausedBtn\n", LOG_START_W);
    std_msgs::String command;

    if (paused) {
        command.data = "resume";
    } else {
        command.data = "pause";
    }

    pauseBtn.setEnabled(false);
    vicControlPub.publish(command);
}

void VictimIdViewWidget::callbackOnlyDtBtn() {
    //LOG("%s callbackOnlyDtBtn\n", LOG_START_W);
    std_msgs::String command;
    command.data = "toggle_dt";
    vicControlPub.publish(command);
}

void VictimIdViewWidget::callbackRightWallBtn() {
    std_msgs::String command;
    command.data = "explorer_mode_right";
    vicControlPub.publish(command);
}

void VictimIdViewWidget::callbackLeftWallBtn() {
    std_msgs::String command;
    command.data = "explorer_mode_left";
    vicControlPub.publish(command);
}

void VictimIdViewWidget::generateFeedback(int type, string fieldStart) {
    // Produce feedback
    string text = fieldStart + ": " + data[type];
    statusLabels[type]->setText(QString(text.c_str()));
    statusCircles[type]->setColour(colour[type]);
}

VictimIdViewPanel::VictimIdViewPanel(ConfigElementPtr config) :
        Panel(config),
        widget()
{}

VictimIdViewPanel::~VictimIdViewPanel() {
}

void VictimIdViewPanel::configure(ConfigElementPtr config) {
    widget.configure(config);
}

QWidget *VictimIdViewPanel::getWidget() {
    return &widget;
}

void VictimIdViewPanel::start() {
    widget.start();
}

void VictimIdViewPanel::stop() {
    widget.stop();
}

} // namespace gui
} // namespace casualty
