//    ROS node that provides the anchor positions from the yaml file
//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <string>
#include <vector>

#include <ros/package.h>

#include "d_fall_pps/Anchors.h"

#include "UWBManagerService.h"

#include "DataListener.h"

#define UWB_UPDATE_DISABLE      0
#define UWB_UPDATE_ENABLE       1
#define UWB_UPDATE_ANCHORS      5
#define UWB_CALIBRATE_ANCHORS   7

using namespace d_fall_pps;

bool enableUWB;
bool calSuccess = false;
UWBAnchorArray anchors;
DataListener selfLoc;

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "UWBManagerService");
    ros::NodeHandle nodeHandle("~");
    
    readYaml();

    ros::ServiceServer uwbService = nodeHandle.advertiseService("UWBData", getAnchorPositions);

    ros::Subscriber uwbUpdate = nodeHandle.subscribe("/my_GUI/UWBUpdate", 1, updateCallback);

    ros::spin();

    return 0;
}

void d_fall_pps::loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
    if(!nodeHandle.getParam(name, val)){
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    if(val.size() != length) {
        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
    }
}

void d_fall_pps::readYaml()
{
    ros::NodeHandle nodeHandle("~");
    int length;
    if(!nodeHandle.getParam("anchorLength", length))
    {
        ROS_ERROR("Failed to get Anchors");
        return;
    }

    anchors.length = length;
    anchors.data.clear();
    
    std::vector<float> anchorArr;
    UWBAnchor tempAnchor;

    for(int i = 0; i < length; i++)
    {
        loadParameterFloatVector(nodeHandle, "anchor" + std::to_string(i), anchorArr, 3);

        tempAnchor.id = i;
        tempAnchor.x = anchorArr[0];
        tempAnchor.y = anchorArr[1];
        tempAnchor.z = anchorArr[2];

        anchors.data.push_back(tempAnchor);
    }

    return;
}

bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response& response)
{
    selfLoc.setOffset(request.x, request.y, request.z);

    if(request.invert != 0)
    {
        for(int i = 0; i < 6; ++i)
        {
            if(request.invert & (1 << i))
                selfLoc.invertAnchor(i);
        }
    }

    readYaml();

    response.enableUWB = enableUWB;
    response.anchorArray = anchors;
    response.calSuccess = calSuccess;

    return true;
}

void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
{
    switch(message.data)
    {
    case UWB_UPDATE_DISABLE:
        enableUWB = false;
        break;
    case UWB_CALIBRATE_ANCHORS:
        calibrateAnchors();
        readYaml();
        break;
    case UWB_UPDATE_ENABLE:
        enableUWB = true;
    case UWB_UPDATE_ANCHORS:
        readYaml();
        break;
    default:
        ROS_WARN("[UWBManagerService] unknown update command");
    }
    
    ROS_WARN("[UWBManagerService] Updated UWB Settings!");
}

void d_fall_pps::calibrateAnchors() 
{
    ROS_WARN("[UWBManagerService] Calibrating Anchors...");

    calSuccess = selfLoc.startAnchorLocalistaion();

    if(calSuccess)
        ROS_WARN("[UWBManagerService] Calibration successfull!");
    else
        ROS_ERROR("[UWBManagerService] Calibration failed!");
}