#include "lfr_state_machine.h"

// for string delimiter
vector<string> LFR_StateMachine::split (string s, string delimiter) const {
    size_t pos_start = 0, pos_end, delim_len = delimiter.length();
    string token;
    vector<string> res;

    while ((pos_end = s.find (delimiter, pos_start)) != string::npos) {
        token = s.substr (pos_start, pos_end - pos_start);
        pos_start = pos_end + delim_len;
        res.push_back (token);
    }

    res.push_back (s.substr (pos_start));
    return res;
}

void LFR_StateMachine::sanitize (string& s) const
{
        char r = '\r', n = '\n';
        s.erase(std::remove(s.begin(), s.end(), n), s.end());
        s.erase(std::remove(s.begin(), s.end(), r), s.end());
}

bool LFR_StateMachine::checkStringValidity (const std::vector<std::string>& s) const
{
    if(s.size() != 5)
    {
        return false;
    }

    try
    {
        for(int i = 0; i < 4; i ++)
        {
            if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
            {
                return false;
            }
        }
        int i = stoi(s[4]);
        if(i != 0 && i != 1)
        {
            return false;
        }
    }
    catch(const std::exception& e) {return false;}
    return true;
}

void LFR_StateMachine::parseString(string s)
{
    sanitize(s);
    std::vector<std::string> splitStr = split(s, ";");
    if(!checkStringValidity(splitStr))
    {
        std::cout<<"Invalid String" << std::endl;
        return;
    }
    double wheels[4] = {0.0, 0.0, 0.0, 0.0};
    int mode = std::stoi(splitStr[4]);
    for(int i = 0; i < 3; i++)
    {
        wheels[i] = std::stod(splitStr[i]);
    }
    if(mode == 0) {
        setState(State::Manual::getInstance());
        uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]);
    }
    else if (mode == 1) {setState(State::Autonomous::getInstance());}
    return;
}

LFR_StateMachine::LFR_StateMachine():    
    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
        return false;
    }),
    uartCommunicator(),
    socket([&](std::exception const &ex)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
        return false;
    })
{
    // Connect String parser to socket
    socket.addListener([&](LFR_Socket::LFR_Telegram telegram)
    {
        std::unique_lock<std::mutex> lock(mutex);
        parseString(std::string(telegram));
    }, &mutex);
    socket.startLoop();
    
    currentState = &State::Idle::getInstance();
    currentState->enter(this);

    //Start the permanent loop
    char input;
    std::cout << "press q to quit" << std::endl;
    std::cin >> input;
    std::cout << "binned" << std::endl;
    while (input != 'q')
    {
        std::cin >> input;
        std::cout << "binned" << std::endl;
    }
    std::cout << "Exiting central" << std::endl;
}

void LFR_StateMachine::setState(LFR_IState& newState)
{
    if (&newState != currentState)
    {
        currentState->exit(this);
        currentState = &newState;
        currentState->enter(this);
    }
    else
    {
        std::cout << "no switch" << std::endl;
    }

}