/*
 * remote_control — 终端 WASD 遥控
 *
 * 用法: SSH 到小车后直接运行 ./remote_control
 *   W/S = 前进/后退
 *   A/D = 左转/右转
 *   Q   = 退出
 *
 * 硬件:
 *   左电机: pwmchip8/pwm2, GPIO12 方向, GPIO13 IN2=高
 *   右电机: pwmchip8/pwm1, GPIO13 方向
 *   使能:   GPIO73
 *   舵机:   pwmchip1/pwm0, 3ms 周期, 1.5ms 中位
 */
#include "MotorController.h"
#include "PwmController.h"
#include "GPIO.h"
#include <cstdio>
#include <cstdlib>
#include <termios.h>
#include <unistd.h>

static constexpr unsigned int MOTOR_PERIOD_NS = 50000;
static constexpr unsigned int SERVO_PERIOD_NS = 3000000;
static constexpr unsigned int SERVO_CENTER_NS = 1500000;
static constexpr unsigned int SERVO_RANGE_NS  = 300000;

int main()
{
    GPIO mortorEN(73);
    mortorEN.setDirection("out");
    mortorEN.setValue(1);

    GPIO leftIn2(13);
    leftIn2.setDirection("out");
    leftIn2.setValue(1);

    MotorController motorL(8, 2, 12, MOTOR_PERIOD_NS);
    MotorController motorR(8, 1, 13, MOTOR_PERIOD_NS);

    PwmController servo(1, 0);
    servo.setPeriod(SERVO_PERIOD_NS);
    servo.setDutyCycle(SERVO_CENTER_NS);
    servo.enable();

    struct termios old_tio, new_tio;
    tcgetattr(STDIN_FILENO, &old_tio);
    new_tio = old_tio;
    new_tio.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &new_tio);

    printf("WASD 遥控已启动\n");
    printf("  W=前进  S=后退  A=左转  D=右转  Q=退出  松手即停\n");

    double speed = 0, steer = 0;
    bool running = true;

    while (running) {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(STDIN_FILENO, &fds);
        struct timeval tv = {0, 50000};

        if (select(STDIN_FILENO + 1, &fds, nullptr, nullptr, &tv) > 0) {
            char c;
            if (read(STDIN_FILENO, &c, 1) == 1) {
                switch (c) {
                case 'w': case 'W': speed =  50; break;
                case 's': case 'S': speed = -50; break;
                case 'a': case 'A': steer = -100; break;
                case 'd': case 'D': steer =  100; break;
                case 'q': case 'Q': running = false; break;
                }
            }
        } else {
            speed = 0;
            steer = 0;
        }

        unsigned int duty_ns = SERVO_CENTER_NS + (unsigned int)(steer / 100.0 * SERVO_RANGE_NS);
        servo.setDutyCycle(duty_ns);
        motorL.updateduty(speed);
        motorR.updateduty(speed);
    }

    motorL.updateduty(0);
    motorR.updateduty(0);
    servo.setDutyCycle(SERVO_CENTER_NS);
    mortorEN.setValue(0);
    tcsetattr(STDIN_FILENO, TCSANOW, &old_tio);
    printf("\n已退出\n");
    return 0;
}