#include <iostream>
#include <vector>
#include <string>
#include <chrono>
#include <thread>
#include <cmath>
#include <algorithm>
#include <csignal>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <SDL2/SDL.h>

// Global flag for signal handling
volatile std::sig_atomic_t g_running = 1;

void signalHandler(int signum) {
    (void)signum;
    g_running = 0;
}

// Modbus RTU CRC16 calculation
uint16_t calcCRC16(const uint8_t* data, size_t len) {
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; ++i) {
        crc ^= data[i];
        for (int j = 0; j < 8; ++j) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

class SerialPort {
private:
    int fd_;
    std::string port_name_;

public:
    SerialPort() : fd_(-1) {}
    ~SerialPort() { closePort(); }

    bool openPort(const std::string& port_name, int baudrate = 115200) {
        port_name_ = port_name;
        fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd_ < 0) {
            std::cerr << "[오류] C++ 포트 열기 실패: " << port_name << std::endl;
            return false;
        }

        fcntl(fd_, F_SETFL, 0);

        struct termios options;
        tcgetattr(fd_, &options);

        speed_t speed = B115200;
        switch (baudrate) {
            case 9600: speed = B9600; break;
            case 19200: speed = B19200; break;
            case 38400: speed = B38400; break;
            case 57600: speed = B57600; break;
            default: speed = B115200; break;
        }

        cfsetispeed(&options, speed);
        cfsetospeed(&options, speed);

        options.c_cflag &= ~PARENB;   // No parity
        options.c_cflag &= ~CSTOPB;   // 1 stop bit
        options.c_cflag &= ~CSIZE;
        options.c_cflag |= CS8;       // 8 data bits
        options.c_cflag &= ~CRTSCTS;  // No hardware flow control
        options.c_cflag |= CREAD | CLOCAL;

        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
        options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
        options.c_oflag &= ~OPOST;

        options.c_cc[VMIN] = 0;
        options.c_cc[VTIME] = 1;  // 0.1s timeout

        tcsetattr(fd_, TCSANOW, &options);
        tcflush(fd_, TCIOFLUSH);
        return true;
    }

    void closePort() {
        if (fd_ >= 0) {
            close(fd_);
            fd_ = -1;
        }
    }

    bool writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
        uint8_t pkt[8];
        pkt[0] = slave;
        pkt[1] = 0x06;
        pkt[2] = (reg >> 8) & 0xFF;
        pkt[3] = reg & 0xFF;
        pkt[4] = (val >> 8) & 0xFF;
        pkt[5] = val & 0xFF;

        uint16_t crc = calcCRC16(pkt, 6);
        pkt[6] = crc & 0xFF;
        pkt[7] = (crc >> 8) & 0xFF;

        tcflush(fd_, TCIOFLUSH);
        ssize_t written = write(fd_, pkt, 8);
        if (written != 8) return false;

        if (slave == 0) return true; // Broadcast

        uint8_t rx_buf[8];
        ssize_t rx_bytes = 0;
        auto start = std::chrono::steady_clock::now();
        while (rx_bytes < 8) {
            ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
            if (res > 0) rx_bytes += res;
            auto now = std::chrono::steady_clock::now();
            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50) break;
            std::this_thread::sleep_for(std::chrono::microseconds(500));
        }
        return rx_bytes == 8;
    }

    bool writeRegs(uint8_t slave, uint16_t reg, const std::vector<uint16_t>& vals) {
        size_t count = vals.size();
        size_t pkt_len = 7 + count * 2 + 2;
        std::vector<uint8_t> pkt(pkt_len);

        pkt[0] = slave;
        pkt[1] = 0x10;
        pkt[2] = (reg >> 8) & 0xFF;
        pkt[3] = reg & 0xFF;
        pkt[4] = (count >> 8) & 0xFF;
        pkt[5] = count & 0xFF;
        pkt[6] = static_cast<uint8_t>(count * 2);

        for (size_t i = 0; i < count; ++i) {
            pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
            pkt[8 + i * 2] = vals[i] & 0xFF;
        }

        uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
        pkt[pkt_len - 2] = crc & 0xFF;
        pkt[pkt_len - 1] = (crc >> 8) & 0xFF;

        tcflush(fd_, TCIOFLUSH);
        ssize_t written = write(fd_, pkt.data(), pkt_len);
        if (written != static_cast<ssize_t>(pkt_len)) return false;

        if (slave == 0) return true;

        uint8_t rx_buf[8];
        ssize_t rx_bytes = 0;
        auto start = std::chrono::steady_clock::now();
        while (rx_bytes < 8) {
            ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
            if (res > 0) rx_bytes += res;
            auto now = std::chrono::steady_clock::now();
            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50) break;
            std::this_thread::sleep_for(std::chrono::microseconds(500));
        }
        return rx_bytes == 8;
    }

    bool readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t>& out_vals) {
        uint8_t pkt[8];
        pkt[0] = slave;
        pkt[1] = 0x03;
        pkt[2] = (reg >> 8) & 0xFF;
        pkt[3] = reg & 0xFF;
        pkt[4] = (count >> 8) & 0xFF;
        pkt[5] = count & 0xFF;

        uint16_t crc = calcCRC16(pkt, 6);
        pkt[6] = crc & 0xFF;
        pkt[7] = (crc >> 8) & 0xFF;

        tcflush(fd_, TCIOFLUSH);
        if (write(fd_, pkt, 8) != 8) return false;

        size_t expected_bytes = 5 + count * 2;
        std::vector<uint8_t> rx_buf(expected_bytes);
        size_t rx_bytes = 0;

        auto start = std::chrono::steady_clock::now();
        while (rx_bytes < expected_bytes) {
            ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
            if (res > 0) rx_bytes += res;
            auto now = std::chrono::steady_clock::now();
            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 15) break;
            std::this_thread::sleep_for(std::chrono::microseconds(100));
        }

        if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
            out_vals.resize(count);
            for (uint16_t i = 0; i < count; ++i) {
                out_vals[i] = (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
            }
            return true;
        }
        return false;
    }
};

class MotorDriver {
private:
    SerialPort* port_;
    uint8_t slave_id_;

public:
    MotorDriver(SerialPort* port, uint8_t slave_id) : port_(port), slave_id_(slave_id) {}

    bool initDriver(uint16_t acl_ms = 500, uint16_t dcl_ms = 500) {
        port_->writeReg(slave_id_, 0x200E, 0x0006); // Alarm clear
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        port_->writeReg(slave_id_, 0x200E, 0x0007); // Disable
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        port_->writeReg(slave_id_, 0x200D, 3);      // Velocity mode
        std::this_thread::sleep_for(std::chrono::milliseconds(30));

        port_->writeRegs(slave_id_, 0x2080, {acl_ms, acl_ms}); // Accel
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        port_->writeRegs(slave_id_, 0x2082, {dcl_ms, dcl_ms}); // Decel
        std::this_thread::sleep_for(std::chrono::milliseconds(30));

        port_->writeReg(slave_id_, 0x200E, 0x0008); // Enable
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        setBrakes(true);
        return true;
    }

    void setBrakes(bool lock) {
        uint16_t val = lock ? 1 : 0;
        port_->writeReg(slave_id_, 0x201A, val);
        port_->writeReg(slave_id_, 0x201B, val);
    }

    void setRPMs(float l_rpm, float r_rpm) {
        int16_t l_val = static_cast<int16_t>(std::clamp(l_rpm, -3000.0f, 3000.0f));
        int16_t r_val = static_cast<int16_t>(std::clamp(r_rpm, -3000.0f, 3000.0f));
        port_->writeRegs(slave_id_, 0x2088, {static_cast<uint16_t>(l_val), static_cast<uint16_t>(r_val)});
    }

    bool readFeedback(float& l_fb, float& r_fb, int32_t& l_tick, int32_t& r_tick) {
        std::vector<uint16_t> regs;
        if (port_->readRegs(slave_id_, 0x20A7, 6, regs)) {
            uint32_t val_l = ((static_cast<uint32_t>(regs[0]) & 0xFFFF) << 16) | (regs[1] & 0xFFFF);
            l_tick = (val_l < 0x80000000U) ? static_cast<int32_t>(val_l) : static_cast<int32_t>(val_l - 0x100000000ULL);

            uint32_t val_r = ((static_cast<uint32_t>(regs[2]) & 0xFFFF) << 16) | (regs[3] & 0xFFFF);
            r_tick = (val_r < 0x80000000U) ? static_cast<int32_t>(val_r) : static_cast<int32_t>(val_r - 0x100000000ULL);

            int16_t vl = static_cast<int16_t>(regs[4]);
            int16_t vr = static_cast<int16_t>(regs[5]);
            l_fb = static_cast<float>(vl);
            r_fb = static_cast<float>(vr);
            return true;
        }
        return false;
    }
};

float applyDeadzone(float val, float deadzone = 0.1f) {
    if (std::abs(val) < deadzone) return 0.0f;
    float sign = (val > 0.0f) ? 1.0f : -1.0f;
    float norm = (std::abs(val) - deadzone) / (1.0f - deadzone);
    // 선형 커브 (Linear Curve): 토크 저하 없이 스틱 조작에 100% 즉각 출력을 보장
    return sign * norm;
}

int main(int argc, char** argv) {
    std::signal(SIGINT, signalHandler);
    std::signal(SIGTERM, signalHandler);

    std::string port1 = "/dev/ttyUSB0";
    std::string port2 = "";
    uint8_t id1 = 1;
    uint8_t id2 = 2;
    bool bcast_mode = false;
    float max_v = 0.30f;          // m/s
    float max_spin_v = 0.30f;     // m/s
    float track_width = 0.576f;   // 좌우 바퀴 중심 거리 576mm
    float wheelbase = 0.3684f;    // 전후 바퀴 중심 거리 368.4mm
    float wheel_radius = 0.131517f; // 사용자 실측 정밀 10.35인치 휠 반지름 (131.517mm)
    float accel_rate = 120.0f;    // RPM/s
    float decel_rate = 180.0f;    // RPM/s (역기전력 서지 방지 및 소프트 정지 튜닝)
    float bumper_offset = 0.045f; // 바퀴 축에서 로봇 맨 앞 범퍼까지의 거리 (45mm)

    for (int i = 1; i < argc; ++i) {
        std::string arg = argv[i];
        if (arg == "--port" && i + 1 < argc) port1 = argv[++i];
        else if (arg == "--port1" && i + 1 < argc) port1 = argv[++i];
        else if (arg == "--port2" && i + 1 < argc) port2 = argv[++i];
        else if (arg == "--id1" && i + 1 < argc) id1 = static_cast<uint8_t>(std::stoi(argv[++i]));
        else if (arg == "--id2" && i + 1 < argc) id2 = static_cast<uint8_t>(std::stoi(argv[++i]));
        else if (arg == "--track_width" && i + 1 < argc) track_width = std::stof(argv[++i]);
        else if (arg == "--wheelbase" && i + 1 < argc) wheelbase = std::stof(argv[++i]);
        else if (arg == "--radius" && i + 1 < argc) wheel_radius = std::stof(argv[++i]);
        else if (arg == "--bumper" && i + 1 < argc) bumper_offset = std::stof(argv[++i]);
        else if (arg == "--accel" && i + 1 < argc) accel_rate = std::stof(argv[++i]);
        else if (arg == "--decel" && i + 1 < argc) decel_rate = std::stof(argv[++i]);
        else if (arg == "--bcast" || arg == "--broadcast") bcast_mode = true;
    }

    constexpr float PI_VAL = 3.14159265358979323846f;
    float rpm_per_ms = 60.0f / (2.0f * PI_VAL * wheel_radius);
    
    // 4WD 유효 선회 계수 (Track Width 576mm, Wheelbase 368.4mm 정밀 기하학)
    float effective_w = std::sqrt(track_width * track_width + wheelbase * wheelbase);
    float k_skid = (track_width + (wheelbase * wheelbase) / track_width) / 2.0f;

    std::cout << "\n==================================================================\n";
    std::cout << " [C++17 4WD 정밀 직진/감속 보정] ZLAC8015D 4모터 동기화 시스템\n";
    std::cout << "  - 좌우 바퀴 거리 (Track Width): " << track_width * 1000.0f << " mm\n";
    std::cout << "  - 전후 바퀴 거리 (Wheelbase)  : " << wheelbase * 1000.0f << " mm\n";
    std::cout << "  - 범퍼 오프셋 (Bumper Offset) : " << bumper_offset * 1000.0f << " mm\n";
    std::cout << "  - 실시간 직진 엔코더 자동 수평 보정 (Straight Auto-Correction) 활성화\n";
    std::cout << "==================================================================\n";

    if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
        std::cerr << "[오류] SDL 조이스틱 초기화 실패: " << SDL_GetError() << std::endl;
        return 1;
    }

    if (SDL_NumJoysticks() < 1) {
        std::cerr << "[경고] 무선 Xbox 컨트롤러(조이스틱)가 연결되지 않았습니다.\n";
        SDL_Quit();
        return 1;
    }

    SDL_Joystick* joystick = SDL_JoystickOpen(0);
    if (!joystick) {
        std::cerr << "[오류] 조이스틱 열기 실패!\n";
        SDL_Quit();
        return 1;
    }
    std::cout << "[정보] C++ Xbox 컨트롤러 연결 성공: " << SDL_JoystickName(joystick) << std::endl;

    SerialPort sp1, sp2;
    if (!sp1.openPort(port1)) return 1;
    std::cout << "[정보] RS485 포트1 (" << port1 << ") 오픈 성공.\n";

    SerialPort* sp2_ptr = &sp1;
    if (!port2.empty()) {
        if (sp2.openPort(port2)) {
            sp2_ptr = &sp2;
            std::cout << "[정보] RS485 포트2 (" << port2 << ") 오픈 성공.\n";
        }
    }

    MotorDriver driver_front(&sp1, bcast_mode ? 0 : id1);
    driver_front.initDriver(150, 150);

    MotorDriver* driver_rear_ptr = nullptr;
    MotorDriver driver_rear(sp2_ptr, bcast_mode ? 0 : id2);
    if (!bcast_mode) {
        driver_rear.initDriver(150, 150);
        driver_rear_ptr = &driver_rear;
    }

    std::cout << "\n[정보] C++ 100Hz 초저지연 루프를 시작합니다. (종료: B 버튼 또는 Ctrl+C)\n\n";

    float target_fl = 0.0f, target_fr = 0.0f;
    float target_rl = 0.0f, target_rr = 0.0f;

    float cmd_fl = 0.0f, cmd_fr = 0.0f;
    float cmd_rl = 0.0f, cmd_rr = 0.0f;

    const float loop_hz = 100.0f;
    const float dt = 1.0f / loop_hz;
    const float max_accel_step = accel_rate * dt;
    const float max_decel_step = decel_rate * dt;

    std::string state = "STOPPED";
    bool trip_initialized = false;
    int32_t start_fl = 0, start_fr = 0, start_rl = 0, start_rr = 0;

    // S-Curve 부드러운 가속 보정 람다 함수 (좌/우 모터 대칭 가속 보장)
    auto sCurveStep = [](float current, float target, float max_accel, float max_decel) {
        float diff = target - current;
        if (std::abs(diff) < 0.01f) return target;

        // 속도 절댓값 크기가 커지는 중이면 가속(accel), 줄어드는 중이면 감속(decel) 적용
        bool is_accelerating = std::abs(target) > std::abs(current);
        float step_rate = is_accelerating ? max_accel : max_decel;
        float step = (diff > 0.0f) ? step_rate : -step_rate;

        float ratio = std::clamp(std::abs(diff) / 60.0f, 0.15f, 1.0f);
        float smooth = ratio * ratio * (3.0f - 2.0f * ratio);
        float delta = step * smooth;
        if (std::abs(delta) > std::abs(diff)) return target;
        return current + delta;
    };

    while (g_running) {
        SDL_Event event;
        while (SDL_PollEvent(&event)) {
            if (event.type == SDL_JOYBUTTONDOWN) {
                if (event.jbutton.button == 0) { // A 버튼 누르면 거리 0m 리셋
                    trip_initialized = false;
                } else if (event.jbutton.button == 1 || event.jbutton.button == 6) { // B or Back
                    g_running = 0;
                }
            }
        }

        Sint16 lx_raw = SDL_JoystickGetAxis(joystick, 0);
        Sint16 ly_raw = SDL_JoystickGetAxis(joystick, 1);
        Sint16 rx_raw = SDL_JoystickGetAxis(joystick, 3);

        float ly = applyDeadzone(-static_cast<float>(ly_raw) / 32767.0f);
        float lx = applyDeadzone(static_cast<float>(lx_raw) / 32767.0f);
        float rx = applyDeadzone(static_cast<float>(rx_raw) / 32767.0f);

        // 직진 제어 락 (Straight-Drive Lock): 스틱 좌우 15% 이내 기울임은 100% 칼직진 고정!
        if (std::abs(lx) < 0.15f) {
            lx = 0.0f;
        }
        if (std::abs(rx) < 0.15f) {
            rx = 0.0f;
        }

        float v_x = ly * max_v; // 선속도 m/s
        float omega = 0.0f;     // 각속도 rad/s

        if (std::abs(rx) > 0.0f) {
            // 제자리 회전 (Spin Turn)
            omega = -rx * (max_spin_v / (effective_w / 2.0f));
            float v_l = -omega * (effective_w / 2.0f);
            float v_r = +omega * (effective_w / 2.0f);

            target_fl = v_l * rpm_per_ms;
            target_fr = -v_r * rpm_per_ms;
            target_rl = v_l * rpm_per_ms;
            target_rr = -v_r * rpm_per_ms;
        } else {
            // 직진 및 커브 차동 선회 (Kinematics)
            omega = -lx * (max_v / (effective_w / 2.0f));
            float v_l = v_x - omega * k_skid;
            float v_r = v_x + omega * k_skid;

            target_fl = v_l * rpm_per_ms;
            target_fr = -v_r * rpm_per_ms;
            target_rl = v_l * rpm_per_ms;
            target_rr = -v_r * rpm_per_ms;
        }

        if (state == "STOPPED") {
            if (std::abs(target_fl) > 0.1f || std::abs(target_fr) > 0.1f) {
                driver_front.setBrakes(false);
                if (driver_rear_ptr) driver_rear_ptr->setBrakes(false);
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
                state = "RUNNING";
            }
        } else if (state == "RUNNING") {
            cmd_fl = sCurveStep(cmd_fl, target_fl, max_accel_step, max_decel_step);
            cmd_fr = sCurveStep(cmd_fr, target_fr, max_accel_step, max_decel_step);
            cmd_rl = sCurveStep(cmd_rl, target_rl, max_accel_step, max_decel_step);
            cmd_rr = sCurveStep(cmd_rr, target_rr, max_accel_step, max_decel_step);

            if (std::abs(target_fl) < 0.1f && std::abs(target_fr) < 0.1f &&
                std::abs(cmd_fl) < 0.5f && std::abs(cmd_fr) < 0.5f) {
                state = "STOPPING";
            }
        } else if (state == "STOPPING") {
            // 감속 시에는 즉각적으로 정지되도록 빠르게 감속 적용 (슬라이딩 감속 지연 제거)
            cmd_fl = sCurveStep(cmd_fl, target_fl, max_decel_step, max_decel_step);
            cmd_fr = sCurveStep(cmd_fr, target_fr, max_decel_step, max_decel_step);
            cmd_rl = sCurveStep(cmd_rl, target_rl, max_decel_step, max_decel_step);
            cmd_rr = sCurveStep(cmd_rr, target_rr, max_decel_step, max_decel_step);

            if (std::abs(cmd_fl) < 0.1f && std::abs(cmd_fr) < 0.1f) {
                cmd_fl = 0.0f; cmd_fr = 0.0f;
                cmd_rl = 0.0f; cmd_rr = 0.0f;
                driver_front.setRPMs(0, 0);
                if (driver_rear_ptr) driver_rear_ptr->setRPMs(0, 0);
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
                driver_front.setBrakes(true);
                if (driver_rear_ptr) driver_rear_ptr->setBrakes(true);
                state = "STOPPED";
            }
        }

        auto comm_start = std::chrono::high_resolution_clock::now();

        if (state != "STOPPED") {
            driver_front.setRPMs(cmd_fl, cmd_fr);
            if (driver_rear_ptr) driver_rear_ptr->setRPMs(cmd_rl, cmd_rr);
        }

        float fl_fb = 0, fr_fb = 0, rl_fb = 0, rr_fb = 0;
        int32_t fl_tick = 0, fr_tick = 0, rl_tick = 0, rr_tick = 0;

        driver_front.readFeedback(fl_fb, fr_fb, fl_tick, fr_tick);
        if (driver_rear_ptr) driver_rear_ptr->readFeedback(rl_fb, rr_fb, rl_tick, rr_tick);

        auto comm_end = std::chrono::high_resolution_clock::now();
        float comm_ms = std::chrono::duration<float, std::milli>(comm_end - comm_start).count();

        // 16384 Ticks = 4배 배율 4채널 엔코더 1회전 (0.798m) 정밀 반영
        float meters_per_tick = (2.0f * PI_VAL * wheel_radius) / 16384.0f;
        if (!trip_initialized && (fl_tick != 0 || fr_tick != 0)) {
            start_fl = fl_tick; start_fr = fr_tick;
            start_rl = rl_tick; start_rr = rr_tick;
            trip_initialized = true;
        }

        int32_t delta_fl = std::abs(fl_tick - start_fl);
        int32_t delta_fr = std::abs(fr_tick - start_fr);
        int32_t delta_rl = std::abs(rl_tick - start_rl);
        int32_t delta_rr = std::abs(rr_tick - start_rr);

        float dist_fl = delta_fl * meters_per_tick;
        float dist_fr = delta_fr * meters_per_tick;
        float dist_rl = delta_rl * meters_per_tick;
        float dist_rr = delta_rr * meters_per_tick;
        float dist_axle = (dist_fl + dist_fr + dist_rl + dist_rr) / 4.0f;
        float dist_bumper = dist_axle + (dist_axle > 0.001f ? bumper_offset : 0.0f);


        printf("\r\033[K[%-8s] AXLE: %5.3fm | BUMPER: %5.3fm | Ticks: FL=%+5d FR=%+5d | LATENCY: %4.1fms",
               state.c_str(), dist_axle, dist_bumper, delta_fl, delta_fr, comm_ms);
        fflush(stdout);

        std::this_thread::sleep_for(std::chrono::microseconds(static_cast<int>(dt * 1000000)));
    }

    std::cout << "\n\n[정보] C++ 4WD 안전 정지 및 브레이크 잠금 처리 중...\n";
    driver_front.setRPMs(0, 0);
    if (driver_rear_ptr) driver_rear_ptr->setRPMs(0, 0);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    driver_front.setBrakes(true);
    if (driver_rear_ptr) driver_rear_ptr->setBrakes(true);

    if (joystick) SDL_JoystickClose(joystick);
    SDL_Quit();

    std::cout << "[정보] C++ 4WD 제어 프로그램이 성공적으로 종료되었습니다.\n";
    return 0;
}