//
// 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"

constexpr float MAX_SPEED = 100.0f;

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;
    networking::Networking net;
    std::atomic<bool> monitorSTM{false};
    std::atomic<bool> monitorUSS{false};
    std::atomic<float> speed{0.0f};
    std::mutex outputMutex;
    UltrasonicSensor sensor{readDevId()};
    car::StreamMeanFilter streamMeanFilter{5};
    auto timer = networking::time::Timer::create(net);
    auto conn = Connection::create(net);
    constexpr float brakeSpeed = -0.03f * 4;
    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";
        }, [] {});

    sensor.init();
    timer = networking::time::Timer::create(net);
    timer->startPeriodicTimeout(
        std::chrono::milliseconds(UltrasonicSensor::DELAY),
        [&]
        {
            auto originalDistance = sensor.getDistance();
            auto filteredDistance = streamMeanFilter.moveWindow(originalDistance);
            if (filteredDistance <= speed * 20)
                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))
    {
        if (in.empty())
        {
            speed = brakeSpeed;
            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")
        {
            value /= 100.0f;
            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";
            }
            speed = value;
            std::cout << "Setting speed to: " << value << "\n";
        }
        else
        {
            std::cout << "Error: Unknown command!\n";
            speed = brakeSpeed;
        }
    }

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