#define _POSIX_C_SOURCE 200809L

#include "mightywatt_controller.h"

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

static void sleep_ms(int ms) {
    struct timespec ts;
    if (ms < 0) {
        ms = 0;
    }
    ts.tv_sec = ms / 1000;
    ts.tv_nsec = (long)(ms % 1000) * 1000000L;
    nanosleep(&ts, NULL);
}

int main(int argc, char **argv) {
    mw_controller *controller = NULL;
    mw_controller_config config;
    mw_controller_snapshot snapshot;
    uint64_t cmd_id = 0;
    int i;

    if (argc < 2) {
        fprintf(stderr, "usage: %s /dev/ttyACM0\n", argv[0]);
        return 2;
    }

    mw_controller_config_init(&config);
    config.port_path = argv[1];
    config.poll_interval_ms = 500;
    config.reconnect_ms = 1500;

    if (mw_controller_start(&controller, &config) != 0) {
        fprintf(stderr, "controller start failed\n");
        return 1;
    }

    for (i = 0; i < 20; ++i) {
        if (mw_controller_get_snapshot(controller, &snapshot) != 0) {
            fprintf(stderr, "snapshot failed\n");
            mw_controller_stop(controller);
            return 1;
        }
        printf("[%02d] state=%s connected=%s pending=%zu error=%s\n",
               i,
               mw_controller_connection_state_name(snapshot.connection_state),
               snapshot.connected ? "yes" : "no",
               snapshot.pending_commands,
               snapshot.last_error[0] ? snapshot.last_error : "-");
        if (snapshot.connected) {
            break;
        }
        sleep_ms(250);
    }

    if (!snapshot.connected) {
        fprintf(stderr, "device did not connect in time\n");
        mw_controller_stop(controller);
        return 1;
    }

    if (mw_controller_queue_load_on(controller, MW_MODE_CURRENT, 250, &cmd_id) != 0) {
        fprintf(stderr, "queue load-on failed\n");
        mw_controller_stop(controller);
        return 1;
    }
    printf("queued command id=%" PRIu64 "\n", cmd_id);

    for (;;) {
        if (mw_controller_wait_for_update(controller, snapshot.snapshot_seq, 2000, NULL) < 0) {
            fprintf(stderr, "wait for update failed\n");
            break;
        }
        if (mw_controller_get_snapshot(controller, &snapshot) != 0) {
            fprintf(stderr, "snapshot failed\n");
            break;
        }
        if (snapshot.last_completed_command_id >= cmd_id) {
            printf("command result=%d completed=%" PRIu64 " state=%s\n",
                   snapshot.last_completed_result,
                   snapshot.last_completed_command_id,
                   mw_controller_connection_state_name(snapshot.connection_state));
            if (snapshot.app_state_valid && snapshot.app_state.report_valid) {
                printf("I=%.3f A V=%.3f V remote=%s status=%u\n",
                       snapshot.app_state.last_report.current_ma / 1000.0,
                       snapshot.app_state.last_report.voltage_mv / 1000.0,
                       snapshot.app_state.last_report.remote ? "on" : "off",
                       (unsigned)snapshot.app_state.last_report.status);
            }
            break;
        }
    }

    mw_controller_queue_safe(controller, NULL);
    sleep_ms(400);
    mw_controller_stop(controller);
    return 0;
}