//
// Created by philipp on 10.05.18.
//

#include <iostream>
#include "../include/VeloxProtocolLib/Connection.h"

constexpr float MAX_SPEED = 1.0f;

int main(int argc, char ** argv)
{
    using namespace veloxProtocol;
    using namespace std::chrono_literals;

    if (argc < 4)
    {
        std::cerr << "Usage: speed angle time[s]\n";
        return -1;
    }

    float speed = atof(argv[1]);
    if (std::abs(speed) > MAX_SPEED)
    {
        speed = speed >= 0 ? MAX_SPEED : -MAX_SPEED;
        std::cout << "WARNING: Speed set to 1.0 due to risk of collision!\n";
    }
    float angle = atof(argv[2]);
    int time = atoi(argv[3]);

    networking::Networking net;
    std::atomic<bool> running{true};
    auto conn = Connection::create(net);
    conn->open(
        "/dev/ttySAC0",
        []
        {},
        []
        {});

    conn->setSpeed(speed);
    conn->setSteeringAngle(angle);

    auto timer = networking::time::Timer::create(net);
    timer->startTimeout(
        std::chrono::seconds{time},
        [&]
        {
            conn->setSpeed(0.0f);
            running = false;
        });

    while (running);
    sleep(1);
    return 0;
}