#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
{
    try
    {
        int i = stoi(s[0]);
        if(i == 0 && s.size() == 5)
        {
            for(int i = 0; i < 4; i ++)
            {
                if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
                {
                    return false;
                }
            }
        }
        else if(i == 1)
        {
            return true;
        }
        else
        {
            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<<"Error: Invalid String: " << s << std::endl;
        return;
    }
    else
    {
        std::cout<< s << std::endl;
    }
    double wheels[4] = {0.0, 0.0, 0.0, 0.0};
    int mode = std::stoi(splitStr[0]);
    if(mode == 0) {
        for(int i = 1; i <= 4; i++)
        {
            wheels[i-1] = std::stod(splitStr[i]);
        }
        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, maxSpeed, [&](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);

    cv::VideoWriter writer = cv::VideoWriter("video_200.avi", cv::VideoWriter::fourcc('M','J','P','G'), 4.0, cv::Size(videoWidth, videoHeight), true);
    
    auto t_start = std::chrono::high_resolution_clock::now();
    auto t_end = std::chrono::high_resolution_clock::now();
    double dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
    
    while(dur < 60000)
    {
        t_end = std::chrono::high_resolution_clock::now();
        dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
        {
            this_thread::sleep_for(std::chrono::milliseconds(50));
            std::unique_lock<std::mutex> lock(imgMutex);
            if(!this->image.empty())
            {
                writer.write(this->image);
            }
        }
    }
    writer.release();
    std::cout << "Exiting central" << std::endl;
}

void LFR_StateMachine::setState(LFR_IState& newState)
{
    if (&newState != currentState)
    {
        currentState->exit(this);
        currentState = &newState;
        currentState->enter(this);
    }
}

void LFR_StateMachine::enterAutonomous()
{
    //cv::Mat img;
    autonomousMode.addListener([&](LFR_Result result)
    {
        std::unique_lock<std::mutex> lock(mutex);
        if (!result.rawImage.empty())
        {
            /*
            img = result.rawImage;
            //Resize to minimize latency using raspi over ssh/adapt to 3.5in screen
            cv::resize(result.rawImage, img, cv::Size(480, 320));

            //Calculate frame rate
            now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
            unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
            double delta = static_cast<double>(deltaMs) / 1000.0;
            double frameRate = 1.0 / static_cast<double>(delta);*/
            double frameRate = -1.0;
            {
                std::unique_lock<std::mutex> lock(imgMutex);
                image = result.rawImage;
            }

            if (result.validLane)
            {
                std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
                    << " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
                    << " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
                uartCommunicator.sendTelegram(result.motorSignals[0], result.motorSignals[1], result.motorSignals[2], result.motorSignals[3]);
            }
            else
            {
                std::cout << "No lane found." << std::endl;;
            }
            //last = now;
        }
    }, &mutex);

    autonomousMode.startLoop();
}

void LFR_StateMachine::exitAutonomous()
{
    autonomousMode.removeListener(&mutex);
    autonomousMode.endLoop();
}