/*
 * gamepad_controller.c — Gamepad/joystick teleop over UDP or KCP
 *
 * Uses the Linux joystick API (/dev/input/js*).
 * Zero external dependencies.
 *
 * Xbox controller mapping (xpad driver):
 *   Left stick Y  (axis 1) → linear.x  (forward/back, inverted)
 *   Left stick X  (axis 0) → linear.y  (strafe)
 *   Right stick X (axis 3) → angular.z (turn)
 *   Button A (0) → emergency stop
 *   Button B (1) → quit
 *
 * Build:  gcc -Wall -O2 -I../common -o gamepad_controller gamepad_controller.c -lm
 * Usage:  ./gamepad_controller [-i IP] [-p PORT] [-d /dev/input/js0]
 *                              [-l MAX_LIN] [-a MAX_ANG] [-z DEADZONE]
 *                              [-t udp|kcp] [-s SERVER] [-r RELAY]
 *                              [-I PEER_ID] [-T TARGET_PEER]
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <math.h>
#include <errno.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <getopt.h>
#include <linux/joystick.h>

#include "../common/protocol.h"
#include "../common/teleop_transport.h"

/* ── config ─────────────────────────────────────────────────────────── */
#define MAX_AXES    16
#define MAX_BUTTONS 16
#define JS_AXIS_MAX 32767.0f

/* Xbox mapping indices */
#define AXIS_LX  0      /* left stick X  → strafe  */
#define AXIS_LY  1      /* left stick Y  → fwd/back (inverted) */
#define AXIS_RX  3      /* right stick X → turn     */

#define BTN_STOP 0      /* A → emergency stop */
#define BTN_QUIT 1      /* B → quit */

static volatile sig_atomic_t g_running = 1;

static void sigint_handler(int sig) { (void)sig; g_running = 0; }

static int parse_port(const char *text, int *port_out)
{
    char *end = NULL;
    long value = strtol(text, &end, 10);

    if (end == text || *end != '\0' || value < 1 || value > 65535)
        return -1;

    *port_out = (int)value;
    return 0;
}

/* ── apply deadzone ─────────────────────────────────────────────────── */
static float apply_deadzone(float v, float dz)
{
    if (fabsf(v) < dz) return 0.0f;
    /* rescale so the output starts from 0 just outside the deadzone */
    float sign = (v > 0) ? 1.0f : -1.0f;
    return sign * (fabsf(v) - dz) / (1.0f - dz);
}

/* ── usage ──────────────────────────────────────────────────────────── */
static void usage(const char *prog)
{
    fprintf(stderr,
        "Usage: %s [options]\n"
        "  -i IP       target IP       (default %s)\n"
        "  -p PORT     target port     (default %d)\n"
        "  -d DEVICE   joystick device (default /dev/input/js0)\n"
        "  -l SPEED    max linear  m/s (default 0.5)\n"
        "  -a SPEED    max angular rad/s (default 0.5)\n"
        "  -z DZ       deadzone 0<=DZ<1 (default 0.1)\n"
        "  -t MODE     transport mode udp|kcp (default udp)\n"
        "  -s ADDR     KCP server addr        (default %s)\n"
        "  -r ADDR     KCP relay addr         (default none)\n"
        "  -I ID       local KCP peer id      (default %s)\n"
        "  -T ID       target KCP peer id     (default %s)\n"
        "  -h          show help\n",
        prog, DEFAULT_IP, DEFAULT_PORT,
        DEFAULT_KCP_SERVER_ADDR,
        DEFAULT_KCP_GAMEPAD_PEER_ID,
        DEFAULT_KCP_TARGET_PEER_ID);
}

/* ──────────────────────────────────────────────────────────────────── */
int main(int argc, char *argv[])
{
    char  ip[64]     = DEFAULT_IP;
    int   port       = DEFAULT_PORT;
    char  device[128] = "/dev/input/js0";
    char  kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
    char  kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
    char  peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_GAMEPAD_PEER_ID;
    char  target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
    float max_lin    = 0.5f;
    float max_ang    = 0.5f;
    float deadzone   = 0.1f;
    teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
    teleop_transport_t transport;
    teleop_transport_config_t transport_config;

    int opt;
    while ((opt = getopt(argc, argv, "i:p:d:l:a:z:t:s:r:I:T:h")) != -1) {
        switch (opt) {
        case 'i': strncpy(ip, optarg, sizeof(ip)-1); ip[sizeof(ip)-1] = '\0'; break;
        case 'p':
            if (parse_port(optarg, &port) != 0) {
                fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
                return 1;
            }
            break;
        case 'd': strncpy(device, optarg, sizeof(device)-1); device[sizeof(device)-1] = '\0'; break;
        case 'l': max_lin  = strtof(optarg, NULL);        break;
        case 'a': max_ang  = strtof(optarg, NULL);        break;
        case 'z': deadzone = strtof(optarg, NULL);        break;
        case 't':
            if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
                fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
                return 1;
            }
            break;
        case 's': strncpy(kcp_server, optarg, sizeof(kcp_server)-1); kcp_server[sizeof(kcp_server)-1] = '\0'; break;
        case 'r': strncpy(kcp_relay, optarg, sizeof(kcp_relay)-1); kcp_relay[sizeof(kcp_relay)-1] = '\0'; break;
        case 'I': strncpy(peer_id, optarg, sizeof(peer_id)-1); peer_id[sizeof(peer_id)-1] = '\0'; break;
        case 'T': strncpy(target_peer, optarg, sizeof(target_peer)-1); target_peer[sizeof(target_peer)-1] = '\0'; break;
        default:  usage(argv[0]); return (opt == 'h') ? 0 : 1;
        }
    }

    if (deadzone < 0.0f || deadzone >= 1.0f) {
        fprintf(stderr, "Invalid deadzone %.3f: expected 0 <= dz < 1\n", deadzone);
        return 1;
    }

    signal(SIGINT, sigint_handler);

    /* ── open joystick ───────────────────────────────────────────── */
    int jsfd = open(device, O_RDONLY | O_NONBLOCK);
    if (jsfd < 0) {
        fprintf(stderr, "Cannot open %s: %s\n"
                "  Hint: connect Xbox controller, check 'ls /dev/input/js*'\n",
                device, strerror(errno));
        return 1;
    }

    char js_name[128] = "Unknown";
    ioctl(jsfd, JSIOCGNAME(sizeof(js_name)), js_name);

    int num_axes = 0, num_buttons = 0;
    ioctl(jsfd, JSIOCGAXES, &num_axes);
    ioctl(jsfd, JSIOCGBUTTONS, &num_buttons);

    printf("========================================\n");
    printf("  Gamepad Teleop Controller\n");
    printf("========================================\n");
    printf("  Device : %s\n", device);
    printf("  Name   : %s\n", js_name);
    printf("  Axes   : %d   Buttons: %d\n", num_axes, num_buttons);
    printf("  Transport: %s\n", teleop_transport_mode_name(transport_mode));
    if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
        printf("  KCP server: %s\n", kcp_server);
        if (kcp_relay[0] != '\0')
            printf("  Relay via : %s\n", kcp_relay);
        printf("  Peer ID   : %s -> %s\n", peer_id, target_peer);
    } else {
        printf("  Target : %s:%d\n", ip, port);
    }
    printf("  Linear : %.2f m/s   Angular: %.2f rad/s\n", max_lin, max_ang);
    printf("  Deadzone: %.2f\n", deadzone);
    printf("----------------------------------------\n");
    printf("  Left stick  → forward/back + strafe\n");
    printf("  Right stick → turn\n");
    printf("  A button    → emergency stop\n");
    printf("  B button    → quit\n");
    printf("========================================\n\n");

    memset(&transport_config, 0, sizeof(transport_config));
    transport_config.mode = transport_mode;
    transport_config.udp_ip = ip;
    transport_config.udp_port = port;
    transport_config.server_addr = kcp_server;
    transport_config.relay_via = kcp_relay;
    transport_config.peer_id = peer_id;
    transport_config.target_peer = target_peer;

    if (teleop_transport_open(&transport, &transport_config) != 0) {
        close(jsfd);
        return 1;
    }

    /* ── state ───────────────────────────────────────────────────── */
    float axes[MAX_AXES];
    int   buttons[MAX_BUTTONS];
    memset(axes, 0, sizeof(axes));
    memset(buttons, 0, sizeof(buttons));

    twist_cmd_t cmd;
    twist_cmd_zero(&cmd);

    struct timeval last_send;
    gettimeofday(&last_send, NULL);

    int e_stop = 0;

    /* ── main loop ───────────────────────────────────────────────── */
    while (g_running) {
        /* read all pending joystick events */
        struct js_event ev;
        while (read(jsfd, &ev, sizeof(ev)) == sizeof(ev)) {
            ev.type &= ~JS_EVENT_INIT;   /* strip init flag */
            if (ev.type == JS_EVENT_AXIS && ev.number < MAX_AXES) {
                axes[ev.number] = (float)ev.value / JS_AXIS_MAX;
            } else if (ev.type == JS_EVENT_BUTTON && ev.number < MAX_BUTTONS) {
                buttons[ev.number] = ev.value;
                if (ev.number == BTN_QUIT && ev.value) {
                    g_running = 0;
                    break;
                }
                if (ev.number == BTN_STOP && ev.value) {
                    e_stop = !e_stop;
                    if (e_stop)
                        printf("\r  ** EMERGENCY STOP **                   ");
                    else
                        printf("\r  ** E-STOP released **                  ");
                    fflush(stdout);
                }
            }
        }
        /* EAGAIN is expected in non-blocking mode */
        if (errno != EAGAIN && errno != 0) {
            perror("read joystick");
            break;
        }
        errno = 0;

        /* map axes → twist (skip if e-stopped) */
        if (e_stop) {
            twist_cmd_zero(&cmd);
        } else {
            float lx_raw = apply_deadzone(-axes[AXIS_LY], deadzone); /* Y inverted */
            float ly_raw = apply_deadzone(-axes[AXIS_LX], deadzone);
            float az_raw = apply_deadzone(-axes[AXIS_RX], deadzone);

            cmd.lx = lx_raw * max_lin;
            cmd.ly = ly_raw * max_lin;
            cmd.lz = 0.0f;
            cmd.ax = 0.0f;
            cmd.ay = 0.0f;
            cmd.az = az_raw * max_ang;
        }

        /* rate-limit sending */
        struct timeval now;
        gettimeofday(&now, NULL);
        long elapsed = (now.tv_sec - last_send.tv_sec) * 1000000
                     + (now.tv_usec - last_send.tv_usec);
        if (elapsed < SEND_INTERVAL_US) {
            usleep(5000);   /* 5 ms sleep to avoid busy-spin */
            continue;
        }
        last_send = now;

        teleop_transport_send_twist(&transport, &cmd);

        printf("\r  cmd: lx=%+.2f ly=%+.2f az=%+.2f  |  raw: LY=%+.2f LX=%+.2f RX=%+.2f   ",
               cmd.lx, cmd.ly, cmd.az,
               axes[AXIS_LY], axes[AXIS_LX], axes[AXIS_RX]);
        fflush(stdout);
    }

    /* send final stop */
    twist_cmd_zero(&cmd);
    teleop_transport_send_twist(&transport, &cmd);

    close(jsfd);
    teleop_transport_close(&transport);
    printf("\nStopped.\n");
    return 0;
}