/*
 * encoder_brake_demo — 编码器刹车测试 v2
 *
 * 编码器2 (gpio67脉冲/gpio72方向) → 控制后面两个轮子同时反转
 * 左后轮: pwmchip8/pwm2, gpio12
 * 右后轮: pwmchip8/pwm1, gpio13
 * 使能:   gpio73
 */
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <fcntl.h>
#include <unistd.h>
#include <ctime>
#include <csignal>

static volatile bool running = true;
void sig_handler(int) { running = false; }

static void write_file(const char *path, const char *val) {
    int fd = open(path, O_WRONLY);
    if (fd >= 0) { write(fd, val, strlen(val)); close(fd); }
}

static int read_pin(const char *path) {
    int fd = open(path, O_RDONLY);
    char v = '0';
    if (fd >= 0) { read(fd, &v, 1); close(fd); }
    return (v == '1') ? 1 : 0;
}

static long now_ns() {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ts.tv_sec * 1000000000L + ts.tv_nsec;
}

int main() {
    signal(SIGINT, sig_handler);
    signal(SIGTERM, sig_handler);

    printf("=== Encoder Brake Demo v2 ===\n");
    printf("编码器: gpio67(脉冲) gpio72(方向)\n");
    printf("左后轮: gpio12 pwm2 | 右后轮: gpio13 pwm1\n");
    printf("掰任意后轮 → 两轮同时反向制动\n\n");

    write_file("/sys/class/gpio/gpio73/value", "1");

    write_file("/sys/class/gpio/gpio12/direction", "out");
    write_file("/sys/class/gpio/gpio13/direction", "out");
    write_file("/sys/class/gpio/gpio67/direction", "in");
    write_file("/sys/class/gpio/gpio72/direction", "in");

    write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
    write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");

    const long WINDOW_NS = 100000000L;        // 100ms 测速窗口
    const int  MAX_DUTY  = 10000;             // 最大占空比 20%
    const double SCALE   = 10.0;               // 速度 → 占空比 缩放系数

    int  prev_lsb  = read_pin("/sys/class/gpio/gpio67/value");
    int  pulse_cnt = 0;
    long t0        = now_ns();
    int  brake_duty = 0;
    long last_print = 0;
    int  last_dir   = read_pin("/sys/class/gpio/gpio72/value");

    printf("%-8s %8s %8s %6s %8s %s\n",
           "raw_pulse", "speed", "duty_ns", "enc_dir", "mot_dir", "");

    while (running) {
        int cur_lsb = read_pin("/sys/class/gpio/gpio67/value");
        int cur_dir = read_pin("/sys/class/gpio/gpio72/value");

        if (cur_lsb != prev_lsb) {
            pulse_cnt++;
            prev_lsb = cur_lsb;
        }
        if (cur_dir != last_dir) last_dir = cur_dir;

        long t1 = now_ns();
        long dt = t1 - t0;

        if (dt >= WINDOW_NS) {
            double speed = pulse_cnt / (dt / 1e9);

            if (speed > 0.5) {
                brake_duty = (int)(speed * SCALE);
                if (brake_duty > MAX_DUTY) brake_duty = MAX_DUTY;

                const char *mdir = cur_dir ? "0" : "1";
                write_file("/sys/class/gpio/gpio12/value", mdir);
                write_file("/sys/class/gpio/gpio13/value", mdir);
            } else {
                brake_duty = 0;
            }

            char buf[16];
            snprintf(buf, sizeof(buf), "%d", brake_duty);
            write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", buf);
            write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", buf);

            if (t1 - last_print > 300000000L) {
                printf("%-8d %8.1f %8d %6d %8s\r",
                       pulse_cnt, speed, brake_duty, cur_dir,
                       cur_dir ? "rev" : "fwd");
                fflush(stdout);
                last_print = t1;
            }

            pulse_cnt = 0;
            t0 = t1;
        }
    }

    write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
    write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");
    write_file("/sys/class/gpio/gpio73/value", "0");
    printf("\n=== Demo stopped ===\n");
    return 0;
}