#include <boost/asio.hpp>
#include <iostream>
#include <thread>
#include <mutex>
#include <string>
struct motor_info
{
    int     Xenc;
    int     Xerr;
    int     sgt_val;
    int     sgt_stat;
    int     is_moving;
    int     control_status;
    float   hdg;
    int     deviation_warn;
    int     temp;
    int     humid;
    int     fan_pwm;
};
class SerialPort {
public:
    SerialPort(const std::string& port, unsigned int baud_rate)
        : io_service_(), serial_(io_service_) {
        boost::system::error_code ec;

        serial_.open(port, ec);
        if (ec) {
            throw std::runtime_error("Failed to open port: " + ec.message());
        }

        setOption(boost::asio::serial_port_base::baud_rate(baud_rate), "baud_rate");
        setOption(boost::asio::serial_port_base::character_size(8), "character_size");
        setOption(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none), "parity");
        setOption(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one), "stop_bits");
        setOption(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none), "flow_control");
    }

    void startReading() {
        read();
    }

    void sendCommand(std::string cmd) {
        write(cmd);
    }

    void write(const std::string& data) {
        boost::asio::async_write(serial_, boost::asio::buffer(data),
            [](const boost::system::error_code& error, std::size_t) {
                if (error) {
                    std::cerr << "Write failed: " << error.message() << std::endl;
                }
            }
        );
    }

    void run() {
        io_service_.run();
    }

    void stop() {
        io_service_.stop();
    }

    void set_controller_info(motor_info& inf) {
        std::lock_guard<std::mutex> lg(mut);
        motorcontroller_information = inf;
    }

    motor_info get_controller_info() {
        std::lock_guard<std::mutex> lg(mut);
        return motorcontroller_information;
    }

    bool parser(std::string& message)
    {

        if (message[0] == '$') {
            //std::cout << message;
            std::vector<std::string> values;
            std::string delimiter = ";";
            size_t pos = 0;
            while ((pos = message.find(delimiter)) != std::string::npos) {
                values.push_back(message.substr(0, pos));
                message.erase(0, pos + delimiter.length());
            }
            //for (int i = 0; i < values.size(); i++) {
            //    std::cout << values[i] << std::endl;
            //}
            if (values.size() == 12) {
                motor_info tmp_info;
                tmp_info.Xenc = stoi(values[1]);
                tmp_info.Xerr = stoi(values[2]);
                tmp_info.sgt_val = stoi(values[3]);
                tmp_info.sgt_stat = stoi(values[4]);
                tmp_info.is_moving = stoi(values[5]);
                tmp_info.control_status = stoi(values[6]);
                tmp_info.hdg = stof(values[7]);
                tmp_info.deviation_warn = stoi(values[8]);
                tmp_info.humid = stoi(values[9]);
                tmp_info.temp = stoi(values[10]);
                tmp_info.fan_pwm = stoi(values[11]);
                set_controller_info(tmp_info);
                return true;
            }
            return false;
            //else
                //std::cout << values.size() << std::endl;
        }
    }

private:
    std::mutex mut;
    motor_info motorcontroller_information;

    template <typename Option>
    void setOption(const Option& option, const std::string& option_name) {
        boost::system::error_code ec;
        serial_.set_option(option, ec);
        if (ec) {
            throw std::runtime_error("Failed to set " + option_name + ": " + ec.message());
        }
    }
    void read() {
        boost::asio::async_read_until(serial_, buffer_, '\n',
            [this](const boost::system::error_code& error, std::size_t bytes_transferred) {
                if (!error) {
                    std::istream is(&buffer_);
                    std::string line;
                    std::getline(is, line);
                    //std::cout << "Received: " << line << std::endl;
                    parser(line);
                    read(); // Continue reading
                }
                else {
                    std::cerr << "Read failed: " << error.message() << std::endl;
                }
            }
        );
    }

    boost::asio::io_service io_service_;
    boost::asio::serial_port serial_;
    boost::asio::streambuf buffer_;
};