#include "protocol.h"

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>

#include "drive_control.h"

static volatile bool s_handshake_verified = false;


static bool parse_drive_mode(const char *value, drive_mode_t *mode, const char **next)
{
    if (strncmp(value, "ECO:", 4) == 0) {
        *mode = DRIVE_MODE_ECO;
        *next = value + 4;
        return true;
    }
    if (strncmp(value, "NORMAL:", 7) == 0) {
        *mode = DRIVE_MODE_NORMAL;
        *next = value + 7;
        return true;
    }
    if (strncmp(value, "BOOST:", 6) == 0) {
        *mode = DRIVE_MODE_BOOST;
        *next = value + 6;
        return true;
    }
    return false;
}

static bool parse_duty_value(const char *value, uint32_t *result)
{
    char *end = NULL;
    const unsigned long parsed = strtoul(value, &end, 10);
    if (end == value || *end != '\0') {
        return false;
    }
    if (parsed > 1023UL) {
        return false;
    }
    *result = (uint32_t)parsed;
    return true;
}

void protocol_reset_session(void)
{
    s_handshake_verified = false;
}

bool protocol_is_handshake_verified(void)
{
    return s_handshake_verified;
}

bool protocol_build_response(const char *input, char *output, size_t output_size)
{
    if (input == NULL || output == NULL || output_size == 0) {
        return false;
    }

    if (strcmp(input, "HELLO") == 0) {
        s_handshake_verified = true;
        drive_control_set_client_connected(true);
        snprintf(output, output_size, "HELLO:SKATE_V1\n");
        return true;
    }

    if (strncmp(input, "PING:", 5) == 0) {
        snprintf(output, output_size, "PONG:%s\n", input + 5);
        return true;
    }

    // Emergency stop is intentionally allowed before handshake.
    // It only moves the system toward a safer motor state.
    if (strcmp(input, "ESTOP") == 0) {
        drive_control_trigger_estop();
        return false;
    }

    if (!s_handshake_verified) {
        snprintf(output, output_size, "ERR:HANDSHAKE_REQUIRED\n");
        return true;
    }

    if (strcmp(input, "THR:PRESS") == 0) {
        drive_control_handle_throttle_press();
        return false;
    }

    if (strcmp(input, "THR:RELEASE") == 0) {
        drive_control_handle_throttle_release();
        return false;
    }

    if (strcmp(input, "BRK:PRESS") == 0) {
        drive_control_handle_brake_press();
        return false;
    }

    if (strcmp(input, "BRK:RELEASE") == 0) {
        drive_control_handle_brake_release();
        return false;
    }

    if (strcmp(input, "MODE:ECO") == 0) {
        drive_control_set_mode(DRIVE_MODE_ECO);
        return false;
    }

    if (strcmp(input, "MODE:NORMAL") == 0) {
        drive_control_set_mode(DRIVE_MODE_NORMAL);
        return false;
    }

    if (strcmp(input, "MODE:BOOST") == 0) {
        drive_control_set_mode(DRIVE_MODE_BOOST);
        return false;
    }

    if (strncmp(input, "CAL:", 4) == 0) {
        drive_mode_t mode = DRIVE_MODE_NORMAL;
        const char *setting = NULL;
        if (!parse_drive_mode(input + 4, &mode, &setting)) {
            snprintf(output, output_size, "ERR:CAL_MODE\n");
            return true;
        }

        if (strncmp(setting, "MIN=", 4) == 0) {
            uint32_t value = 0U;
            if (!parse_duty_value(setting + 4, &value)) {
                snprintf(output, output_size, "ERR:CAL_VALUE\n");
                return true;
            }
            drive_control_set_mode_pwm_min_duty(mode, value);
            snprintf(output, output_size, "OK:CAL\n");
            return true;
        }

        if (strncmp(setting, "MAX=", 4) == 0) {
            uint32_t value = 0U;
            if (!parse_duty_value(setting + 4, &value)) {
                snprintf(output, output_size, "ERR:CAL_VALUE\n");
                return true;
            }
            drive_control_set_mode_pwm_max_duty(mode, value);
            snprintf(output, output_size, "OK:CAL\n");
            return true;
        }

        snprintf(output, output_size, "ERR:CAL_FIELD\n");
        return true;
    }

    return false;
}