/*
 * keyboard_controller.c - Keyboard teleop over UDP or KCP
 *
 * Keys:
 *   W/Up    forward      S/Down  backward
 *   A/Left  turn left    D/Right turn right
 *   Q       strafe left  E       strafe right
 *   Space   stop
 *   [ / ]   linear speed down/up
 *   - / =   angular speed down/up
 *   Ctrl-C  quit
 *
 * Build:  gcc -Wall -O2 -I../common -o keyboard_controller keyboard_controller.c
 * Usage:  ./keyboard_controller [-i IP] [-p PORT] [-l MAX_LIN] [-a MAX_ANG]
 *                              [-t udp|kcp] [-s SERVER] [-r RELAY]
 *                              [-I PEER_ID] [-T TARGET_PEER]
 */

#include <getopt.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/time.h>
#include <termios.h>
#include <unistd.h>

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

/*
 * Terminals do not provide key-release events, so keep the last motion command
 * alive briefly to bridge the initial auto-repeat delay while a key is held.
 */
#define KEY_HOLD_TIMEOUT_US 500000L

static struct termios g_orig_termios;
static volatile sig_atomic_t g_running = 1;

static long elapsed_us(const struct timeval *start, const struct timeval *end)
{
    return (end->tv_sec - start->tv_sec) * 1000000L
         + (end->tv_usec - start->tv_usec);
}

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;
}

static void restore_terminal(void)
{
    tcsetattr(STDIN_FILENO, TCSANOW, &g_orig_termios);
    printf("\n\033[?25h");
    fflush(stdout);
}

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

static void set_raw_mode(void)
{
    struct termios raw;
    tcgetattr(STDIN_FILENO, &g_orig_termios);
    atexit(restore_terminal);
    raw = g_orig_termios;
    raw.c_lflag &= ~(ICANON | ECHO | ISIG);
    raw.c_cc[VMIN] = 0;
    raw.c_cc[VTIME] = 0;
    tcsetattr(STDIN_FILENO, TCSANOW, &raw);
}

static int read_key(long timeout_us)
{
    fd_set fds;
    struct timeval tv;
    unsigned char c;

    FD_ZERO(&fds);
    FD_SET(STDIN_FILENO, &fds);
    tv.tv_sec = timeout_us / 1000000L;
    tv.tv_usec = timeout_us % 1000000L;

    if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
        return -1;
    if (read(STDIN_FILENO, &c, 1) != 1)
        return -1;

    if (c == 0x1B) {
        unsigned char seq[2];

        tv.tv_sec = 0;
        tv.tv_usec = 20000;
        FD_ZERO(&fds);
        FD_SET(STDIN_FILENO, &fds);
        if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
            return 0x1B;
        if (read(STDIN_FILENO, &seq[0], 1) != 1)
            return 0x1B;

        FD_ZERO(&fds);
        FD_SET(STDIN_FILENO, &fds);
        tv.tv_sec = 0;
        tv.tv_usec = 20000;
        if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
            return 0x1B;
        if (read(STDIN_FILENO, &seq[1], 1) != 1)
            return 0x1B;

        if (seq[0] == '[') {
            switch (seq[1]) {
            case 'A': return 'W';
            case 'B': return 'S';
            case 'D': return 'A';
            case 'C': return 'D';
            default: break;
            }
        }
        return 0x1B;
    }

    if (c >= 'a' && c <= 'z')
        c = (unsigned char)(c - ('a' - 'A'));
    return c;
}

static void print_banner(void)
{
    printf("\033[2J\033[H");
    printf("========================================\n");
    printf("  Keyboard Teleop Controller\n");
    printf("========================================\n");
    printf("  W/Up    : forward      S/Down : back\n");
    printf("  A/Left  : turn left    D/Right: turn right\n");
    printf("  Q       : strafe left  E      : strafe right\n");
    printf("  Space   : stop\n");
    printf("  [ / ]   : linear speed  -/+\n");
    printf("  - / =   : angular speed -/+\n");
    printf("  Ctrl-C  : quit\n");
    printf("========================================\n\n");
}

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"
            "  -l SPEED    max linear  speed m/s   (default 0.5)\n"
            "  -a SPEED    max angular speed rad/s (default 0.5)\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_KEYBOARD_PEER_ID,
            DEFAULT_KCP_TARGET_PEER_ID);
}

int main(int argc, char *argv[])
{
    char ip[64] = DEFAULT_IP;
    int port = DEFAULT_PORT;
    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_KEYBOARD_PEER_ID;
    char target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
    float max_lin = 0.5f;
    float max_ang = 0.5f;
    const float speed_step = 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:l:a: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 'l':
            max_lin = strtof(optarg, NULL);
            break;
        case 'a':
            max_ang = 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;
        }
    }

    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) {
        return 1;
    }

    set_raw_mode();
    signal(SIGINT, sigint_handler);
    print_banner();
    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\n", max_lin, max_ang);
    printf("\033[?25l");

    twist_cmd_t cmd;
    twist_cmd_zero(&cmd);

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

    while (g_running) {
        int key = read_key(SEND_INTERVAL_US);

        if (key >= 0) {
            twist_cmd_zero(&cmd);
            switch (key) {
            case 'W':
                cmd.lx = max_lin;
                gettimeofday(&last_motion_key, NULL);
                break;
            case 'S':
                cmd.lx = -max_lin;
                gettimeofday(&last_motion_key, NULL);
                break;
            case 'A':
                cmd.az = max_ang;
                gettimeofday(&last_motion_key, NULL);
                break;
            case 'D':
                cmd.az = -max_ang;
                gettimeofday(&last_motion_key, NULL);
                break;
            case 'Q':
                cmd.ly = max_lin;
                gettimeofday(&last_motion_key, NULL);
                break;
            case 'E':
                cmd.ly = -max_lin;
                gettimeofday(&last_motion_key, NULL);
                break;
            case ' ':
                break;
            case ']':
                max_lin += speed_step;
                printf("\r  Linear speed: %.2f m/s       ", max_lin);
                fflush(stdout);
                continue;
            case '[':
                max_lin = (max_lin > speed_step) ? max_lin - speed_step : speed_step;
                printf("\r  Linear speed: %.2f m/s       ", max_lin);
                fflush(stdout);
                continue;
            case '=':
                max_ang += speed_step;
                printf("\r  Angular speed: %.2f rad/s    ", max_ang);
                fflush(stdout);
                continue;
            case '-':
                max_ang = (max_ang > speed_step) ? max_ang - speed_step : speed_step;
                printf("\r  Angular speed: %.2f rad/s    ", max_ang);
                fflush(stdout);
                continue;
            case 0x03:
                g_running = 0;
                continue;
            default:
                continue;
            }
        } else {
            struct timeval now;
            gettimeofday(&now, NULL);
            if (elapsed_us(&last_motion_key, &now) > KEY_HOLD_TIMEOUT_US)
                twist_cmd_zero(&cmd);
        }

        {
            struct timeval now;
            long elapsed;

            gettimeofday(&now, NULL);
            elapsed = elapsed_us(&last_send, &now);
            if (elapsed < SEND_INTERVAL_US)
                continue;
            last_send = now;
        }

        teleop_transport_send_twist(&transport, &cmd);

        printf("\r  cmd: lx=%+.2f ly=%+.2f az=%+.2f  |  lin=%.2f ang=%.2f   ",
               cmd.lx, cmd.ly, cmd.az, max_lin, max_ang);
        fflush(stdout);
    }

    twist_cmd_zero(&cmd);
    teleop_transport_send_twist(&transport, &cmd);

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