#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include <inttypes.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>

#include <hexagram/can.h>
#include <hexagram/capture.h>
#include <hexagram/schedule.h>
#include <hexagram/telemetry/dash.h>

#include "dash2can.h"

char *hexagram_arglist_dash2can(void) {
    return "udp-port canif";
}

static void usage(int argc, char **argv, const char *message, ...) {
    if (message) {
        va_list args;

        va_start(args, message);
        vfprintf(stderr, message, args);
         fputc('\n', stderr);
        va_end(args);
    }

    fprintf(stderr, "usage: hexagram %s %s\n", argv[0], hexagram_arglist_dash2can());

    exit(1);
}

static int ev_handler(struct can_frame *frame, void *ctx) {
    hexagram_can_if *can_if = ctx;

    return hexagram_can_if_write(can_if, frame);
}

#define SCHEDULE_SLOT_COUNT 6

static hexagram_schedule_slot table[SCHEDULE_SLOT_COUNT] = {
    { 16670, { .can_id = 0x280, .can_dlc = 8 }, ev_handler },
    { 16670, { .can_id = 0x288, .can_dlc = 8 }, ev_handler },
    { 16670, { .can_id = 0x320, .can_dlc = 8 }, ev_handler },
    { 16670, { .can_id = 0x420, .can_dlc = 8 }, ev_handler },
    { 16670, { .can_id = 0x540, .can_dlc = 8 }, ev_handler },
    { 16670, { .can_id = 0x5a0, .can_dlc = 8 }, ev_handler },
};

static void table_update(hexagram_schedule_slot *table,
                         hexagram_telemetry_dash_packet *packet) {
    float speed_ms = sqrt(powf(packet->velocity.x, 2)
                        + powf(packet->velocity.z, 2));

    uint16_t engine_rpm = (uint16_t)(packet->engine_rpm.current * 4),
             speed_rps  = (uint16_t)(speed_ms * 2.0);

    /* Engine speed */
    table[0].frame.data[3] = (engine_rpm & 0xff00) >> 8;
    table[0].frame.data[2] =  engine_rpm & 0x00ff;

    /* Current gear */
    if (packet->race.gear == 0) {
        table[4].frame.data[7] = 0x30; /* Reverse gear */
    } else {
        table[4].frame.data[7] = 0xc | (packet->race.gear + 2) << 4;
    }

    /* Vehicle speed */
    table[5].frame.data[1] = (speed_rps & 0xff00) >> 8;
    table[5].frame.data[2] =  speed_rps & 0x00ff;
}

int hexagram_main_dash2can(int argc, char **argv) {
    size_t i;
    int sock;

    struct sockaddr_in sockaddr;
    hexagram_can_if *can_if;
    hexagram_schedule *schedule;

    if (argc < 2) {
        usage(argc, argv, "No listening UDP port provided");
    } else if (argc < 3) {
        usage(argc, argv, "No CAN interface name provided");
    } else if (argc > 3) {
        usage(argc, argv, "Too many arguments provided");
    }

    if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
        goto error_socket;
    }

    memset(&sockaddr, '\0', sizeof(sockaddr));
    sockaddr.sin_family      = AF_INET;
    sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    sockaddr.sin_port        = htons(atoi(argv[1]));

    if (bind(sock, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) < 0) {
        goto error_bind;
    }

    if ((can_if = hexagram_can_if_open(argv[2])) == NULL) {
        perror("hexagram_can_if_open()");

        goto error_can_if_open;
    }

    for (i=0; i<SCHEDULE_SLOT_COUNT; i++) {
        memset(table[i].frame.data, '\0', sizeof(table[i].frame.data));
    }

    /* Engine temperature  */
    table[1].frame.data[1] = (uint8_t)((87.0 / 0.75) + 48);

    /* Fuel status */
    table[2].frame.data[2] = 0x0e;

    /* Ambient temperature */
    table[3].frame.data[1] = 141;

    if ((schedule = hexagram_schedule_create(table,
                                             SCHEDULE_SLOT_COUNT,
                                             can_if)) == NULL) {
        goto error_schedule_create;
    }

    hexagram_schedule_run(schedule);

    while (1) {
        hexagram_telemetry_dash_packet packet;
        ssize_t len;

        if ((len = recv(sock, &packet, sizeof(packet), 0)) < 0) {
            goto error_io;
        } else if (len > (ssize_t)sizeof(packet)) {
            continue;
        }

        table_update(table, &packet);
    }

    hexagram_schedule_destroy(schedule);
    hexagram_can_if_close(can_if);

    close(sock);

    return 0;

error_io:
    hexagram_schedule_destroy(schedule);

error_schedule_create:
    hexagram_can_if_close(can_if);

error_can_if_open:
error_bind:
    close(sock);

error_socket:
    return 1;
}