//
// Created by philipp on 26.04.18.
//

#include "../include/VeloxProtocolLib/Connection.h"
#include <iostream>
#include <boost/algorithm/string.hpp>
#include "USS_SRF02.h"

constexpr float MAX_SPEED = 1.0f;

int main(int argc, char ** argv)
{
    using namespace veloxProtocol;
    networking::Networking net;
    std::atomic<bool> monitorSTM{false};
    std::atomic<bool> monitorUSS{false};
    std::atomic<bool> running{true};
    std::mutex outputMutex;
    auto conn = Connection::create(net);
    conn->open(
        "/dev/ttySAC0",
        [&]
        {
            if (!monitorSTM)
                return;

            std::lock_guard<std::mutex> lock{outputMutex};
            std::cout << "[STM Update]\nSpeed=" << conn->getMeasuredSpeed().get()
                      << "\nSteering Angle: " << conn->getMeasuredSteeringAngle().get() << "\n\n";
        },
        []
        { std::cout << "Connection closed!\n"; });

    std::thread ussThread{
        [&]
        {
            USS_SRF02 uss{USS_SRF02::DEVICE_ADDRESS1};

            while (running)
            {
                int distance = uss.getDistance();
                //if (distance > 20)
                    //conn->setSpeed(0);

                if (!monitorUSS)
                    continue;

                std::lock_guard<std::mutex> lock{outputMutex};
                std::cout << "[USS Update]\nDistance=" << distance << "\n\n";
            }
        }};

    std::cout << "\n\nMANUAL\n"
              << "------\n"
              << "Enter 'stm' to enable/disable monitoring for STM data.\n"
              << "Enter 'uss' to enable/disable monitoring for USS data.\n"
              << "Enter 'q' to quit.\n"
              << "Enter 's <float> t' to set speed\n"
              << "Enter 'a <float>' to set angle\n"
              << "\n\n";

    for (std::string in; in != "q"; std::getline(std::cin, in))
    {
        if (in.empty())
            continue;

        if (in == "stm")
        {
            monitorSTM = !monitorSTM;
            continue;
        }
        else if (in == "uss")
        {
            monitorUSS = !monitorUSS;
            continue;
        }

        std::vector<std::string> split;
        boost::split(split, in, [](char c)
        { return c == ' '; });

        std::string cmd = split.at(0);
        float value = std::stof(split.at(1));

        if (cmd == "a")
        {
            std::cout << "Setting steering angle to: " << value << "\n";
            conn->setSteeringAngle(value);
        }
        else if (cmd == "s")
        {
            std::cout << "Setting speed to: " << value << "\n";
            if (std::abs(value) > MAX_SPEED)
            {
                value = value >= 0 ? MAX_SPEED : -MAX_SPEED;
                std::cout << "WARNING: Speed set to 1.0 due to risk of collision!\n";
            }
            conn->setSpeed(value);
        }
        else std::cout << "Error: Unknown command!\n";
    }

    running = false;
    conn->close();
    sleep(1);

    return 0;
}