#include <esp_log.h>
#include <math.h>
#include "mbiface.h"
#include "socket_server.h"
#include "tasks.h"
#include "modbus.h"
#include "settings.h"
#include "actuators.h"

static const char * TAG = "mb";

Tcpd_t g_mbifc_server = NULL;
ModbusSlave_t gModbus = {};

#define IDENT_MAGIC 3334
#define REBOOT_MAGIC 0xB007 /* 45063 */

static uint32_t latch = 0;

enum HoldingRegisters {
  H_IDENT = 0,
  H_CPM = 1,
  H_USV_H_upper = 2, // 2+3 (float 32b)
  H_USV_H_lower = 3,
  H_TOTAL_COUNT_upper = 4, // 4+5 (32b)
  H_TOTAL_COUNT_lower = 5,
  H_TOTAL_USV_upper = 6, // 6+7 (32b)
  H_TOTAL_USV_lower = 7,

  // power source calibration
  H_PWM_FREQ = 10,
  H_PWM_DUTY = 11,
  H_PWM_THRES = 12,
  H_ADC_VAL = 13,

  H_REBOOT = 70,
};

static void restartLater() {
    vTaskDelay(pdMS_TO_TICKS(100));
    esp_restart();
}

/**
 * Socket read handler
 */
static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
{
    uint8_t buf[1024];
    uint8_t buf2[1024];
    int nbytes = read(sockfd, buf, sizeof(buf));
    if (nbytes <= 0) return ESP_FAIL;

    size_t resp_len = 0;
    ModbusError_t e = mb_handleRequest(&gModbus, buf, nbytes, buf2, 1024, &resp_len);
    if (e == 0) {
        tcpd_send(client, buf2, (ssize_t) resp_len);
    } else {
        ESP_LOGE(TAG, "Error %d, closing socket", e);
        tcpd_kick(client);
    }

    return ESP_OK;
}

ModbusException_t startOfAccess(ModbusSlave_t *pSlave, ModbusFunction_t function, uint8_t i) {
    return 0;
}

void endOfAccess(ModbusSlave_t *ms) {
    //
}

union ui16 {
  uint16_t u;
  uint16_t i;
};

union ui32 {
  uint32_t u;
  uint32_t i;
  float f;
};

ModbusException_t rh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t *pValue) {
    ESP_LOGD(TAG, "Read holding %d", ref);

    switch (ref) {
        case H_IDENT:
            *pValue = IDENT_MAGIC;
            break;

        case H_CPM:
            *pValue = (uint16_t) last_cpm;
            break;

        case H_USV_H_upper:
            latch = ((union ui32) { .f = last_usv_h }).u;
            *pValue = (uint16_t) ((latch >> 16) & 0xFFFF);
            break;

        case H_USV_H_lower:
            *pValue = (uint16_t) (latch & 0xFFFF);
            break;

        case H_TOTAL_COUNT_upper:
            latch = count_total;
            *pValue = (uint16_t) ((latch >> 16) & 0xFFFF);
            break;

        case H_TOTAL_COUNT_lower:
            *pValue = (uint16_t) (latch & 0xFFFF);
            break;

        case H_TOTAL_USV_upper:
            latch = ((union ui32) { .f = total_usv }).u;
            *pValue = (uint16_t) ((latch >> 16) & 0xFFFF);
            break;

        case H_TOTAL_USV_lower:
            *pValue = (uint16_t) (latch & 0xFFFF);
            break;

        case H_PWM_FREQ:
            *pValue = (uint16_t) pwm_freq;
            break;

        case H_PWM_DUTY:
            *pValue = (uint16_t) pwm_duty;
            break;

        case H_PWM_THRES:
            *pValue = (uint16_t) pwm_thres;
            break;

        case H_ADC_VAL:
            *pValue = (uint16_t) last_adc_mv;
            break;

        default:
            return 0;
    }

    return MB_EXCEPTION_OK;
}

ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t value) {
    ESP_LOGD(TAG, "Write holding %d := %02x", ref, value);

    switch (ref) {
        case H_PWM_FREQ:
            if (value > 100 && value < 30000) {
                pwm_freq = value;
                gSettings.pwm_freq = value;
                settings_persist(SETTINGS_pwm_freq);
            } else {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            break;

        case H_PWM_DUTY:
            if (value > 1 && value < 950) { // safety margin
                pwm_duty = value;
                gSettings.pwm_duty = value;
                settings_persist(SETTINGS_pwm_duty);
            } else {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            break;

        case H_PWM_THRES:
            pwm_thres = value;
            gSettings.pwm_thres = value;
            settings_persist(SETTINGS_pwm_thres);
            break;

        case H_REBOOT:
            if (value == REBOOT_MAGIC) {
                // if we restart immediately, the modbus req won't finish and master then sends retries
                //  and restarts us again and again
                xTaskCreate(restartLater, "kill", 2048, NULL, PRIO_HIGH, NULL);
            } else {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            break;
        default:
            return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }

    return MB_EXCEPTION_OK;
}

void mbiface_setup()
{
    ESP_LOGI(TAG, "initing MB iface");
    gModbus.proto = MB_PROTO_TCP;
    gModbus.addr = 1;
    gModbus.startOfAccess = startOfAccess;
    gModbus.endOfAccess = endOfAccess;
    gModbus.readHolding = rh;
    gModbus.writeHolding = wh;

    tcpd_config_t server_config = TCPD_INIT_DEFAULT();
    server_config.max_clients = 1;
    server_config.task_prio = MBIFC_TASK_PRIO;
    server_config.task_stack = MBIFC_TASK_STACK;
    server_config.task_name = "MBIFC";
    server_config.port = 502;
    server_config.read_fn = read_fn;

    ESP_ERROR_CHECK(tcpd_init(&server_config, &g_mbifc_server));
}