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

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

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

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

    std::cout << "\n\nMANUAL\n------\nEnter 'm' to enable/disable monitoring.\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;
        else if (in == "m")
            monitoring = !monitoring;
        else if (in == "q")
            break;

        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(2));

        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";
            conn->setSpeed(value);
        }
        else std::cout << "Error: Unknown command!\n";
    }

    conn->close();
    sleep(1);

    return 0;
}