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

#include "../include/VeloxProtocolLib/Connection.h"
#include <iostream>
#include <boost/algorithm/string.hpp>
#include <fstream>
#include "ultrasonic/UltrasonicSensor.h"
#include "tools/StreamMeanFilter.h"
#include "NetworkingLib/DatagramSender.h"

constexpr bool usesPid{true};

constexpr float maxSpeed = usesPid ? 3.0f : 0.1f;
constexpr float brakeSpeed = usesPid ? 0 : -0.06f * 4;

int readDevId()
{
    // open config file
    std::string userHome = getenv("HOME");
    std::ifstream configFile;
    configFile.open(userHome + "/CarConfig/sensor.config", std::ifstream::in);
    if (!configFile.is_open())
    { throw std::runtime_error{"File '/CarConfig/sensor.config' not found!\n"}; }

    // desired parameters
    int devId;
    bool devIdFound{false};
    std::string contentLine;
    while (!configFile.eof())
    {
        std::getline(configFile, contentLine);

        // split this line
        std::istringstream iss(contentLine);
        std::vector<std::string> words{std::istream_iterator<std::string>{iss}, {}};

        if (words.size() > 1)
        { // this line contains parameter[at(0)] and value[at(1)]
            if (words.at(0) == "devId:")
            {
                std::stringstream ss;
                ss << std::hex << words.at(1);
                ss >> devId;
                devIdFound = true;
            }
        }
    }

    if (!devIdFound)
    { throw std::runtime_error{"No attribute devId found!\n"}; }

    configFile.close();

    return devId;
}

int main(int argc, char ** argv)
{
    using namespace veloxProtocol;
    using namespace std::chrono_literals;
    networking::Networking net;
    std::atomic<bool> monitorSTM{false};
    std::atomic<bool> monitorUSS{false};
    std::atomic<float> speed{0.0f};
    std::atomic<float> stopDistance{0};
    std::mutex outputMutex;
    auto sensor = UltrasonicSensor::create(net, readDevId());
    car::StreamMeanFilter streamMeanFilter{5};
    auto timer = networking::time::Timer::create(net);
    auto sender = networking::message::DatagramSender<std::string>::create(net);
    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()
                      << "\nPID Change: " << conn->getMeasuredPIDChange().get() << "\n\n";
        }, [] {});

    timer->startPeriodicTimeout(
        10ms,
        [&]
        {
            std::ostringstream oss;
            oss << std::to_string(conn->getMeasuredSpeed().get()) << "\n"
                << std::to_string(conn->getMeasuredPIDChange().get());
            sender->send(oss.str(), "10.5.33.26", 10150, 1ms);
        });

    sensor->start(
        [&](auto distance)
        {
            auto originalDistance = distance;
            //auto filteredDistance = streamMeanFilter.moveWindow(originalDistance);
            auto filteredDistance = distance;
//            if (filteredDistance <= stopDistance)
//                conn->setSpeed(brakeSpeed);
//            else
                conn->setSpeed(speed);

            if (!monitorUSS)
                return;

            std::lock_guard<std::mutex> lock{outputMutex};
            std::cout << "[USS Update]\n"
                      << "Original Distance=" << originalDistance << "\n"
                      << "Filtered Distance=" << filteredDistance << "\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))
    {
        try
        {
            if (in == "stm")
                monitorSTM = !monitorSTM;
            else if (in == "uss")
                monitorUSS = !monitorUSS;
            else
            {
                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")
                {
                    value /= usesPid ? 1 : 100;
                    if (std::abs(value) > maxSpeed)
                    {
                        value = value >= 0 ? maxSpeed : -maxSpeed;
                        std::cout << "WARNING: Speed set to 1.0 due to risk of collision!\n";
                    }
                    speed = value;
                    stopDistance = usesPid ? 50 + speed.load() * 20
                                           : speed.load() * 100 * 15;
                    std::cout << "Setting speed to: " << speed << "\n"
                              << "Stop Distance is now: " << stopDistance << "\n";
                }
            }
        }
        catch (...)
        {
            std::cout << "Invalid input! The car brakes now!\n";
            speed = brakeSpeed;
        }
    }

    conn->setSpeed(brakeSpeed);
    sleep(1);
    conn->close();
    return 0;
}