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

static const char * TAG = "mb";

Tcpd_t g_mbifc_server = NULL;
ModbusSlave_t gModbus = {};

#define IDENT_MAGIC 3333
#define REBOOT_MAGIC 0xB007 /* 45063 */
#define INPUT_REG_MIRROR_IN_HOLDING_BASE_ADDR 1000

enum HoldingRegisters {
  H_IDENT = 0,
  // Control
  H_MODE = 1,
  H_POWER = 2,
  H_SUMMER_MODE = 3,

  // Settings
  H_INITIAL_MODE = 10,
  H_INITIAL_POWER = 11,
  H_RECUP_MODE = 12,
  H_RECUP_TIME = 13,
  H_RECUP_TIME_MIN = 14,
  H_RECUP_TIME_MAX = 15,
//  H_RECUP_FACTOR = 16,
  H_MIN_POWER = 17,
  H_T_IN_MIN = 18,
  H_T_IN_MAX = 19,
  H_T_STOPDELTA_IN = 20,
  H_T_STOPDELTA_OUT = 21,
  H_T_STOP_SPEED = 22,

  // Hardware settings (don't need to change once set correctly)
  H_RAMP_TIME = 30,
  H_BLIND_TIME = 31,
  H_SWAP_TEMPS = 32,
  H_SWAP_PWMDIR = 33,

  H_REBOOT = 70,
};

enum InputRegisters {
  I_T_VALIDITY = 1,
  I_T_IN_INST = 2,
  I_T_OUT_INST = 3,
  I_T_INDOOR = 4,
  I_T_OUTDOOR = 5,
  I_T_INFLOW = 6,
  I_T_EXHAUST = 7,
  I_RECUP_TIME_IN = 8,
  I_RECUP_TIME_OUT = 9,

  I_MODE_INST = 20,
  I_MOTOR_RAMP = 21,
  I_BLIND_RAMP = 22,
  I_MOTOR_SECS = 23,
  I_MOTOR_HOURS = 24,
  I_UPTIME_SECS = 25,
  I_UPTIME_HOURS = 26,
  I_FREE_HEAP_KB = 27,
};

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;
};

#define cels2reg(cels) (((union ui16) { .i = ((int16_t) roundf((cels) * 100.0f)) }).u)
#define reg2cels(regv) (((float) ((union ui16) { .u = (regv) }).i) / 100.0f)

ModbusException_t ri(ModbusSlave_t *pSlave, uint16_t ref, uint16_t *pValue) {
    ESP_LOGD(TAG, "Read input %d", ref);
    uint16_t scratch16 = 0;
    uint32_t scratch32 = 0;

    switch (ref) {
        case I_RECUP_TIME_IN:
            *pValue = gState.real_recup_time_in;
            break;
        case I_RECUP_TIME_OUT:
            *pValue = gState.real_recup_time_out;
            break;
        case I_T_VALIDITY:
            scratch16 |= (int)gState.valid_t_actual_in;
            scratch16 |= (int)gState.valid_t_actual_out << 1;
            scratch16 |= (int)gState.valid_t_indoor << 2;
            scratch16 |= (int)gState.valid_t_outdoor << 3;
            scratch16 |= (int)gState.valid_t_inflow << 4;
            scratch16 |= (int)gState.valid_t_exhaust << 5;
            *pValue = scratch16;
            break;
        case I_T_IN_INST:
            *pValue = cels2reg(gState.t_actual_in);
            break;
        case I_T_OUT_INST:
            *pValue = cels2reg(gState.t_actual_out);
            break;
        case I_T_INDOOR:
            *pValue = cels2reg(gState.t_indoor);
            break;
        case I_T_OUTDOOR:
            *pValue = cels2reg(gState.t_outdoor);
            break;
        case I_T_INFLOW:
            *pValue = cels2reg(gState.t_inflow);
            break;
        case I_T_EXHAUST:
            *pValue = cels2reg(gState.t_exhaust);
            break;
        case I_MODE_INST:
            *pValue = gState.instantaneous_vent_mode;
            break;
        case I_MOTOR_RAMP:
            *pValue = gState.ramp;
            break;
        case I_BLIND_RAMP:
            *pValue = gState.blind_position;
            break;
        case I_UPTIME_SECS:
            *pValue = gState.uptime_secs;
            break;
        case I_UPTIME_HOURS:
            *pValue = gState.uptime_hours;
            break;
        case I_MOTOR_SECS:
            *pValue = gState.motor_secs;
            break;
        case I_MOTOR_HOURS:
            *pValue = gState.motor_hours;
            break;
        case I_FREE_HEAP_KB:
            scratch32 = esp_get_free_heap_size() / 1024;
            if (scratch32 > UINT16_MAX) {
                scratch32 = UINT16_MAX;
            }
            *pValue = scratch32;
            break;

        default:
            // this allows Bridge reading
            return 0;
            //return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }

    return MB_EXCEPTION_OK;
}

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_MODE:
            *pValue = (int) gState.set_vent_mode;
            break;
        case H_POWER:
            *pValue = gState.set_power;
            break;
        case H_SUMMER_MODE:
            *pValue = (int) gSettings.summer_mode;
            break;
        case H_INITIAL_MODE:
            *pValue = (int) gSettings.initial_mode;
            break;
        case H_INITIAL_POWER:
            *pValue = gSettings.initial_power;
            break;
        case H_RECUP_MODE:
            *pValue = gSettings.recup_mode;
            break;
        case H_RECUP_TIME:
            *pValue = gSettings.recup_time;
            break;
        case H_RECUP_TIME_MIN:
            *pValue = gSettings.min_recup_time;
            break;
        case H_RECUP_TIME_MAX:
            *pValue = gSettings.max_recup_time;
            break;
//        case H_RECUP_FACTOR:
//            *pValue = gSettings.recup_factor;
//            break;

        case H_T_IN_MIN:
            *pValue = cels2reg(gSettings.t_in_min);
            break;
        case H_T_IN_MAX:
            *pValue = cels2reg(gSettings.t_in_max);
            break;
        case H_T_STOPDELTA_IN:
            *pValue = cels2reg(gSettings.t_stopdelta_in);
            break;
        case H_T_STOPDELTA_OUT:
            *pValue = cels2reg(gSettings.t_stopdelta_out);
            break;
        case H_T_STOP_SPEED:
            *pValue = cels2reg(gSettings.t_stop_speed);
            break;

        case H_RAMP_TIME:
            *pValue = gSettings.ramp_time;
            break;
        case H_BLIND_TIME:
            *pValue = gSettings.blind_time;
            break;
        case H_MIN_POWER:
            *pValue = gSettings.min_power;
            break;
        case H_SWAP_TEMPS:
            *pValue = gSettings.swap_temps;
            break;
        case H_SWAP_PWMDIR:
            *pValue = gSettings.swap_pwm_dir;
            break;
        default:
            // inputs are mapped to the holding address space
            if (ref >= INPUT_REG_MIRROR_IN_HOLDING_BASE_ADDR) {
                return ri(pSlave, ref - INPUT_REG_MIRROR_IN_HOLDING_BASE_ADDR, pValue);
            }

            return 0;
            //return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }

    return MB_EXCEPTION_OK;
}

static bool is_valid_vent_mode(int value) {
    switch (value) {
        case VENT_MODE_OFF:
        case VENT_MODE_FREE:
        case VENT_MODE_OUT:
        case VENT_MODE_IN:
        case VENT_MODE_RECUP:
            return true;
        default:
            return false;
    }
}

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

    switch (ref) {
        case H_MODE:
            if (!is_valid_vent_mode(value)) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            fan_set_vent_mode(value);
            break;
        case H_POWER:
            if (value > 100) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            fan_set_power(value);
            break;
        case H_SUMMER_MODE:
            gSettings.summer_mode = (value != 0);
            settings_persist(SETTINGS_summer_mode);
            break;
        case H_INITIAL_MODE:
            if (!is_valid_vent_mode(value)) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.initial_mode = value;
            settings_persist(SETTINGS_initial_mode);
            break;
        case H_INITIAL_POWER:
            if (value > 100) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.initial_power = value;
            settings_persist(SETTINGS_initial_power);
            break;
        case H_RECUP_MODE:
            if (value == RECUP_MODE_TEMP || value == RECUP_MODE_TIME) {
                gSettings.recup_mode = value;
            } else {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            break;
        case H_RECUP_TIME:
            if (value == 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.recup_time = value;
            settings_persist(SETTINGS_recup_time);
            break;
        case H_RECUP_TIME_MIN:
            if (value == 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.min_recup_time = value;
            settings_persist(SETTINGS_min_recup_time);
            break;
        case H_RECUP_TIME_MAX:
            if (value == 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.max_recup_time = value;
            settings_persist(SETTINGS_max_recup_time);
            break;
//        case H_RECUP_FACTOR:
//            if (value > 100) {
//                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
//            }
//            gSettings.recup_factor = value;
//            settings_persist(SETTINGS_recup_factor);
//            break;

        case H_T_IN_MIN:
            f = reg2cels(value);
            gSettings.t_in_min = f;
            settings_persist(SETTINGS_t_in_min);
            break;
        case H_T_IN_MAX:
            f = reg2cels(value);
            gSettings.t_in_max = f;
            settings_persist(SETTINGS_t_in_max);
            break;
        case H_T_STOPDELTA_IN:
            f = reg2cels(value);
            if (f < 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.t_stopdelta_in = f;
            settings_persist(SETTINGS_t_stopdelta_in);
            break;
        case H_T_STOPDELTA_OUT:
            f = reg2cels(value);
            if (f < 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.t_stopdelta_out = f;
            settings_persist(SETTINGS_t_stopdelta_out);
            break;
        case H_T_STOP_SPEED:
            f = reg2cels(value);
            if (f < 0) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.t_stop_speed = f;
            settings_persist(SETTINGS_t_stop_speed);
            break;

        case H_RAMP_TIME:
            gSettings.ramp_time = value;
            settings_persist(SETTINGS_ramp_time);
            break;
        case H_BLIND_TIME:
            gSettings.blind_time = value;
            settings_persist(SETTINGS_blind_time);
            break;
        case H_MIN_POWER:
            if (value > 100) {
                return MB_EXCEPTION_ILLEGAL_DATA_VALUE;
            }
            gSettings.min_power = value;
            settings_persist(SETTINGS_min_power);
            break;
        case H_SWAP_TEMPS:
            gSettings.swap_temps = value;
            settings_persist(SETTINGS_swap_temps);
            break;
        case H_SWAP_PWMDIR:
            gSettings.swap_temps = value;
            settings_persist(SETTINGS_swap_pwm_dir);
            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);
            }
            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;
    gModbus.readInput = ri;

    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));
}