#pragma once
#include <iostream>
#include <algorithm>

#include <lfr.h>
#include <lfr_socket.h>
#include <uart_communication.h>
#include "lfr_state_interface.h"
#include "lfr_states.h"

class LFR_StateMachine
{
    LFR_IState* currentState;

    const int thresholdBinary = 140;
    const int videoHeight = 720;
    const int videoWidth = 1280;
    const int gaussKernelSize = 11;
    const double maxSpeed = 0.5;

    std::mutex mutex;

    LFR autonomousMode;
    LFR_UART uartCommunicator;
    LFR_Socket socket;

    vector<string> split (string s, string delimiter) const;
    void sanitize (string& s) const;
    bool checkStringValidity (const std::vector<std::string>& s) const;
    void parseString(string s);    

public:
    void enterAutonomous();
    void exitAutonomous();
    LFR_StateMachine();
    inline LFR_IState* getCurrentState() const {return currentState;}
    void setState(LFR_IState& newState);
};