#include "protocol.h"
#include "wifi_transport.h"
#include <string.h>
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"

#define CMD_QUEUE_LEN 8

static QueueHandle_t cmd_queue;

/* session state */
static Session sessions[MAX_SESSIONS];
static uint8_t session_count;
static uint8_t current_session_id;

/* ---- 7-bit MIDI encoding ---- */

static void encode_float(float val, uint8_t *out)
{
    uint8_t *p = (uint8_t *)&val;
    out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2) |
             ((p[2] >> 5) & 4) | ((p[3] >> 4) & 8);
    out[1] = p[0] & 0x7F;
    out[2] = p[1] & 0x7F;
    out[3] = p[2] & 0x7F;
    out[4] = p[3] & 0x7F;
}

static void encode_u16(uint16_t val, uint8_t *out)
{
    uint8_t *p = (uint8_t *)&val;
    out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2);
    out[1] = p[0] & 0x7F;
    out[2] = p[1] & 0x7F;
}

void encode_u32(uint32_t val, uint8_t *out)
{
    uint8_t *p = (uint8_t *)&val;
    out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2) |
             ((p[2] >> 5) & 4) | ((p[3] >> 4) & 8);
    out[1] = p[0] & 0x7F;
    out[2] = p[1] & 0x7F;
    out[3] = p[2] & 0x7F;
    out[4] = p[3] & 0x7F;
}

uint32_t decode_u32(const uint8_t *d)
{
    uint8_t b[4];
    b[0] = d[1] | ((d[0] & 1) << 7);
    b[1] = d[2] | ((d[0] & 2) << 6);
    b[2] = d[3] | ((d[0] & 4) << 5);
    b[3] = d[4] | ((d[0] & 8) << 4);
    uint32_t v;
    memcpy(&v, b, 4);
    return v;
}

float decode_float(const uint8_t *d)
{
    uint8_t b[4];
    b[0] = d[1] | ((d[0] & 1) << 7);
    b[1] = d[2] | ((d[0] & 2) << 6);
    b[2] = d[3] | ((d[0] & 4) << 5);
    b[3] = d[4] | ((d[0] & 8) << 4);
    float v;
    memcpy(&v, b, 4);
    return v;
}

uint16_t decode_u16(const uint8_t *d)
{
    uint8_t b[2];
    b[0] = d[1] | ((d[0] & 1) << 7);
    b[1] = d[2] | ((d[0] & 2) << 6);
    uint16_t v;
    memcpy(&v, b, 2);
    return v;
}

/* ---- command queue ---- */

int protocol_init(void)
{
    cmd_queue = xQueueCreate(CMD_QUEUE_LEN, sizeof(Command));
    if (!cmd_queue) return -1;

    session_count = 0;
    current_session_id = 0;
    memset(sessions, 0, sizeof(sessions));
    return 0;
}

int protocol_recv_command(Command *cmd, uint32_t timeout_ms)
{
    TickType_t ticks = (timeout_ms == UINT32_MAX) ? portMAX_DELAY : pdMS_TO_TICKS(timeout_ms);
    return xQueueReceive(cmd_queue, cmd, ticks) == pdTRUE ? 0 : -1;
}

void protocol_push_command(const Command *cmd)
{
    xQueueSend(cmd_queue, cmd, 0);
}

/* ---- session management ---- */

const Session *session_get_all(uint8_t *count)
{
    *count = session_count;
    return sessions;
}

uint8_t session_get_current(void)
{
    return current_session_id;
}

uint8_t session_create(const char *name, uint8_t name_len)
{
    if (session_count >= MAX_SESSIONS)
        return 0xFF;

    uint8_t id = session_count + 1;
    Session *s = &sessions[session_count];
    s->id = id;
    if (name_len > MAX_SESSION_NAME)
        name_len = MAX_SESSION_NAME;
    memcpy(s->name, name, name_len);
    s->name[name_len] = '\0';
    s->active = 1;
    session_count++;
    current_session_id = id;
    return id;
}

int session_switch(uint8_t id)
{
    for (uint8_t i = 0; i < session_count; i++) {
        if (sessions[i].id == id) {
            current_session_id = id;
            return 0;
        }
    }
    return -1;
}

int session_rename(uint8_t id, const char *name, uint8_t name_len)
{
    for (uint8_t i = 0; i < session_count; i++) {
        if (sessions[i].id == id) {
            if (name_len > MAX_SESSION_NAME)
                name_len = MAX_SESSION_NAME;
            memcpy(sessions[i].name, name, name_len);
            sessions[i].name[name_len] = '\0';
            return 0;
        }
    }
    return -1;
}

/* ---- SysEx send ---- */

static int send_sysex(const uint8_t *sysex, uint16_t len)
{
    return wifi_send_sysex(sysex, len);
}

/* ---- outbound: keepalive ---- */

int send_keepalive(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_KEEPALIVE, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

/* ---- outbound: EIS ---- */

int send_sweep_start(uint32_t num_points, float freq_start, float freq_stop,
                     uint32_t ts_ms, uint16_t meas_id)
{
    uint8_t sx[28];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_SWEEP_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    encode_float(freq_start, &sx[p]); p += 5;
    encode_float(freq_stop, &sx[p]); p += 5;
    encode_u32(ts_ms, &sx[p]); p += 5;
    encode_u16(meas_id, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_eis_point(uint16_t index, const EISPoint *pt)
{
    uint8_t sx[64];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_DATA_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(pt->freq_hz,        &sx[p]); p += 5;
    encode_float(pt->mag_ohms,       &sx[p]); p += 5;
    encode_float(pt->phase_deg,      &sx[p]); p += 5;
    encode_float(pt->z_real,         &sx[p]); p += 5;
    encode_float(pt->z_imag,         &sx[p]); p += 5;
    encode_float(pt->rtia_mag_before, &sx[p]); p += 5;
    encode_float(pt->rtia_mag_after,  &sx[p]); p += 5;
    encode_float(pt->rev_mag,        &sx[p]); p += 5;
    encode_float(pt->rev_phase,      &sx[p]); p += 5;
    encode_float(pt->pct_err,        &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_sweep_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_SWEEP_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int send_config(const EISConfig *cfg)
{
    uint8_t sx[32];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CONFIG;
    encode_float(cfg->freq_start_hz, &sx[p]); p += 5;
    encode_float(cfg->freq_stop_hz, &sx[p]); p += 5;
    encode_u16(cfg->points_per_decade, &sx[p]); p += 3;
    sx[p++] = (uint8_t)cfg->rtia;
    sx[p++] = (uint8_t)cfg->rcal;
    sx[p++] = (uint8_t)cfg->electrode;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: LSV ---- */

int send_lsv_start(uint32_t num_points, float v_start, float v_stop,
                   uint32_t ts_ms, uint16_t meas_id)
{
    uint8_t sx[28];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_LSV_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    encode_float(v_start, &sx[p]); p += 5;
    encode_float(v_stop, &sx[p]); p += 5;
    encode_u32(ts_ms, &sx[p]); p += 5;
    encode_u16(meas_id, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_lsv_point(uint16_t index, float v_mv, float i_ua)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_LSV_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(v_mv, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_lsv_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_LSV_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

/* ---- outbound: Amperometry ---- */

int send_amp_start(float v_hold, uint32_t ts_ms, uint16_t meas_id)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_AMP_START;
    encode_float(v_hold, &sx[p]); p += 5;
    encode_u32(ts_ms, &sx[p]); p += 5;
    encode_u16(meas_id, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_amp_point(uint16_t index, float t_ms, float i_ua)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_AMP_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(t_ms, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_amp_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_AMP_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

/* ---- outbound: Chlorine ---- */

int send_cl_start(uint32_t num_points, uint32_t ts_ms, uint16_t meas_id)
{
    uint8_t sx[18];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    encode_u32(ts_ms, &sx[p]); p += 5;
    encode_u16(meas_id, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_cl_point(uint16_t index, float t_ms, float i_ua, uint8_t phase)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(t_ms, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = phase & 0x7F;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_cl_result(float i_free_ua, float i_total_ua)
{
    uint8_t sx[16];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_RESULT;
    encode_float(i_free_ua, &sx[p]); p += 5;
    encode_float(i_total_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_cl_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_CL_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

/* ---- outbound: pH calibration ---- */

int send_ph_cal(float slope, float offset)
{
    uint8_t sx[16];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_PH_CAL;
    encode_float(slope, &sx[p]); p += 5;
    encode_float(offset, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: pH ---- */

int send_ph_result(float v_ocp_mv, float ph, float temp_c,
                   uint32_t ts_ms, uint16_t meas_id)
{
    uint8_t sx[28];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_PH_RESULT;
    encode_float(v_ocp_mv, &sx[p]); p += 5;
    encode_float(ph, &sx[p]); p += 5;
    encode_float(temp_c, &sx[p]); p += 5;
    encode_u32(ts_ms, &sx[p]); p += 5;
    encode_u16(meas_id, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: temperature ---- */

int send_temp(float temp_c)
{
    uint8_t sx[12];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_TEMP;
    encode_float(temp_c, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: cell constant ---- */

int send_cell_k(float k)
{
    uint8_t sx[12];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CELL_K;
    encode_float(k, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: chlorine factor ---- */

int send_cl_factor(float f)
{
    uint8_t sx[12];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_FACTOR;
    encode_float(f, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

/* ---- outbound: reference collection ---- */

int send_ref_frame(uint8_t mode, uint8_t rtia_idx)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_FRAME, mode & 0x7F, rtia_idx & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int send_ref_lp_range(uint8_t mode, uint8_t low_idx, uint8_t high_idx)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_LP_RANGE, mode & 0x7F, low_idx & 0x7F, high_idx & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int send_refs_done(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REFS_DONE, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int send_ref_status(uint8_t has_refs)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_STATUS, has_refs & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

/* ---- outbound: session sync ---- */

int send_session_created(uint8_t id, const char *name, uint8_t name_len)
{
    uint8_t sx[48];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_SESSION_CREATED;
    sx[p++] = id & 0x7F;
    sx[p++] = name_len & 0x7F;
    for (uint8_t i = 0; i < name_len && p < sizeof(sx) - 1; i++)
        sx[p++] = name[i] & 0x7F;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_session_switched(uint8_t id)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_SESSION_SWITCHED, id & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int send_session_list(void)
{
    uint8_t sx[128];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_SESSION_LIST;
    sx[p++] = session_count & 0x7F;
    sx[p++] = current_session_id & 0x7F;
    for (uint8_t i = 0; i < session_count && p < sizeof(sx) - 4; i++) {
        sx[p++] = sessions[i].id & 0x7F;
        uint8_t nlen = (uint8_t)strlen(sessions[i].name);
        sx[p++] = nlen & 0x7F;
        for (uint8_t j = 0; j < nlen && p < sizeof(sx) - 1; j++)
            sx[p++] = sessions[i].name[j] & 0x7F;
    }
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_session_renamed(uint8_t id, const char *name, uint8_t name_len)
{
    uint8_t sx[48];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_SESSION_RENAMED;
    sx[p++] = id & 0x7F;
    sx[p++] = name_len & 0x7F;
    for (uint8_t i = 0; i < name_len && p < sizeof(sx) - 1; i++)
        sx[p++] = name[i] & 0x7F;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int send_client_list(uint8_t count)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_CLIENT_LIST, count & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}