#include <iostream>
#include <optional>
#include <vector>
#include <filesystem>
#include <CppLinuxSerial.h>

class CaaarController {
public:
    CaaarController(const std::string& port)
        : serialPort(port, LibSerial::SerialStreamBuf::BAUD_9600) {
    }


    std::optional<std::string> autoDetect() {
        std::filesystem::path basePath("/dev");

        for (const auto& entry : std::filesystem::directory_iterator(basePath)) {
            const auto& path = entry.path();
            if (path.filename().string().substr(0, 6) == "ttyACM" || path.filename().string().substr(0, 6) == "ttyUSB") {
                serialPort.Open(path);
                if (serialPort.good()) {
                    if (detect()) {
                        return path;
                    }
                    serialPort.Close();
                }
            }
        }

        return std::nullopt;
    }

    bool detect() {
        sendCommand("$");
        std::string response = readResponse();
        return response == "$b29b3875-2e31-4b13-991f-08b7f62c141b";
    }

    void setLEDMode(int mode) {
        sendCommand("l" + std::to_string(mode));
    }

    void setLEDColor(int index, int r, int g, int b) {
        sendCommand("l" + std::to_string(index) + " " + std::to_string(r) + "," + std::to_string(g) + "," + std::to_string(b));
    }

    void setServoDutyCycle(int servo, int dutyCycle) {
        sendCommand("s" + std::to_string(servo) + " " + std::to_string(dutyCycle));
    }

private:
    void sendCommand(const std::string& command) {
        serialPort << command << std::endl;
    }

    std::string readResponse() {
        std::string response;
        std::getline(serialPort, response);
        return response;
    }

    LibSerial::SerialStream serialPort;
};