//
// Created by bisheng on 5/10/22.
//

#ifndef OPENCV_TEST_ROBOTINTERFACE_H
#define OPENCV_TEST_ROBOTINTERFACE_H
#include "Config.h"
#include "ImageMeasurement.h"
#include "MyPLC.h"
#include <mutex>
#include <condition_variable>
#include <queue>
#include <atomic>

void GenerateMovePulseCommand(vector<unsigned char> &move_pulse_command, int x_pulse, int y_pulse);

class RobotInterface {
private:
    ImageMeasurement image_measure_left;
    ImageMeasurement image_measure_right;
    MyPLC my_plc;
    int Calibration_Pulse_;

    enum Task {
        LeftInitialize,
        RightInitialize,
        LeftCheck,
        RightCheck,
        Free
    };
    Task task;
    mutex task_mutex_lock_;
    mutex send_mutex_lock_;
    // 接收数据锁
    std::queue<vector<unsigned char>> command_list;
    mutex receive_data_mutex_lock_;
    condition_variable receive_data_condition_variable_;
    atomic_bool adjust_received;
    atomic_bool adjust_finished;
    atomic_bool vision_finished;

public:
    RobotInterface(int PLC_IP, int Calibration_Pulse):
    Calibration_Pulse_(Calibration_Pulse),
    my_plc(PLC_IP),
    image_measure_left("/dev/left_video", Calibration_Pulse),
    image_measure_right("/dev/right_video", Calibration_Pulse),
    task(Free),
    adjust_received(false),
    adjust_finished(false),
    vision_finished(false)
    {}
    void test_left();
    void test_right();
    void ExecuteThread();
    void ReceiveThread();
    void ParseThread();
};


#endif //OPENCV_TEST_ROBOTINTERFACE_H