#include "onewire_temp.h"

#include <string.h>

#include "debug.h"
#include "onewire.h"
#include "systick.h"

#define ONEWIRE_CONVERSION_TIME_MS 750
#define ONEWIRE_MAX_RETRIES 255

typedef struct {
  uint8_t address[8];
  float temperature;
  onewire_state_t state;
  uint32_t convert_start_time;
  bool valid;
  uint8_t error_count;
  uint32_t last_success_time;
} onewire_sensor_t;

typedef struct {
  onewire_sensor_t sensors[ONEWIRE_MAX_SENSORS];
  uint8_t count;
  bool parallel_mode;
} onewire_system_t;

static onewire_system_t ow_sys = {0};

void onewire_temp_init(void) {
  uint8_t addr[8];

  OneWireBegin();
  OneWireResetSearch();

  while (ow_sys.count < ONEWIRE_MAX_SENSORS) {
    if (!OneWireSearch(addr, true)) {
      break;
    }

    // Validate sensor
    if (OneWireCrc8(addr, 7) != addr[7]) {
      continue;
    }

    // Check if it's a temperature sensor
    switch (addr[0]) {
      case 0x10:  // DS18S20
      case 0x28:  // DS18B20
      case 0x22:  // DS1822
        break;
      default:
        continue;  // Not a supported temperature sensor
    }

    // Store valid sensor
    onewire_sensor_t* sensor = &ow_sys.sensors[ow_sys.count];
    memcpy(sensor->address, addr, 8);
    sensor->valid = true;
    sensor->state = ONEWIRE_STATE_READY;
    sensor->temperature = ONEWIRE_TEMP_INVALID;
    sensor->error_count = 0;
    sensor->last_success_time = millis();
    ow_sys.count++;
  }
}

static float convert_temperature(const uint8_t* data, uint8_t family_code) {
  int16_t raw = (data[1] << 8) | data[0];

  // DS18S20
  if (family_code == 0x10) {
    raw = raw << 3;
    if (data[7] == 0x10) {
      raw = (raw & 0xFFF0) + 12 - data[6];
    }
  }
  // DS18B20/DS1822
  else {
    switch (data[4] & 0x60) {
      case 0x00:
        raw &= ~7;
        break;  // 9-bit
      case 0x20:
        raw &= ~3;
        break;  // 10-bit
      case 0x40:
        raw &= ~1;
        break;  // 11-bit
    }
  }

  return (float)(raw / 16.0);
}

static bool start_conversion(onewire_sensor_t* sensor) {
  if (!sensor->state != ONEWIRE_STATE_READY) {
    return false;
  }

  if (!OneWireReset()) {
    return false;
  }

  OneWireSelect(sensor->address);
  OneWireWrite(0x44, 0);  // Start conversion w/o parasite power
  sensor->state = ONEWIRE_STATE_CONVERTING;
  sensor->convert_start_time = millis();

  return true;
}

static bool read_temperature(onewire_sensor_t* sensor) {
  uint8_t data[9];

  if (!OneWireReset()) {
    return false;
  }

  OneWireSelect(sensor->address);
  OneWireWrite(0xBE, 0);  // Read scratchpad
  OneWireReadBytes(data, 9);

  if (OneWireCrc8(data, 8) != data[8]) {
    return false;
  }

  sensor->temperature = convert_temperature(data, sensor->address[0]);
  sensor->error_count = 0;
  sensor->last_success_time = millis();
  return true;
}

void onewire_temp_process(void) {
  uint32_t now = millis();

  for (uint8_t i = 0; i < ow_sys.count; i++) {
    onewire_sensor_t* sensor = &ow_sys.sensors[i];

    if (!sensor->valid) {
      continue;
    }

    switch (sensor->state) {
      case ONEWIRE_STATE_READY:
        if (!ow_sys.parallel_mode) {
          if (!start_conversion(sensor)) {
            sensor->error_count++;
            if (sensor->error_count >= ONEWIRE_MAX_RETRIES) {
              sensor->valid = false;
              DEBUG_PRINT("sensor %d marked temporarily invalid after %d retries\n", i, ONEWIRE_MAX_RETRIES);
            }
          }
        }
        break;

      case ONEWIRE_STATE_CONVERTING:
        if (now - sensor->convert_start_time >= ONEWIRE_CONVERSION_TIME_MS) {
          sensor->state = ONEWIRE_STATE_READ;
        }
        break;

      case ONEWIRE_STATE_READ:
        if (!read_temperature(sensor)) {
          sensor->error_count++;
          if (sensor->error_count >= ONEWIRE_MAX_RETRIES) {
            sensor->valid = false;
            DEBUG_PRINT("sensor %d marked temporarily invalid after %d retries\n", i, ONEWIRE_MAX_RETRIES);
          }
        } else {
          sensor->valid = true;  // re-enable
          sensor->error_count = 0;
        }
        sensor->state = ONEWIRE_STATE_READY;
        break;
    }
  }
}

void onewire_temp_start_parallel(void) {
  if (!OneWireReset()) {
    return;
  }

  OneWireSkip();          // Address all devices
  OneWireWrite(0x44, 1);  // Start conversion with parasite power

  uint32_t now = millis();
  for (uint8_t i = 0; i < ow_sys.count; i++) {
    if (ow_sys.sensors[i].valid) {
      ow_sys.sensors[i].state = ONEWIRE_STATE_CONVERTING;
      ow_sys.sensors[i].convert_start_time = now;
    }
  }
}

float onewire_temp_get(uint8_t index) {
  return (index < ow_sys.count && ow_sys.sensors[index].valid)
             ? ow_sys.sensors[index].temperature
             : ONEWIRE_TEMP_INVALID;
}

uint8_t onewire_temp_count(void) { return ow_sys.count; }

const uint8_t* onewire_temp_address(uint8_t index) {
  return (index < ow_sys.count) ? ow_sys.sensors[index].address : NULL;
}

bool onewire_temp_valid(uint8_t index) {
  return (index < ow_sys.count) ? ow_sys.sensors[index].valid : false;
}

void onewire_temp_set_parallel(bool enable) { ow_sys.parallel_mode = enable; }

// MQTT
void onewire_temp_publish_discovery(MQTTClient* client, const char* node_id) {
    char topic[MAX_TOPIC_LENGTH];
    char sensor_name[32];
    uint8_t sensor_count = onewire_temp_count();
    
    // append to node list
    size_t current_len = strlen(nodes_list);
    char* ptr = nodes_list + current_len;
    size_t remaining = sizeof(nodes_list) - current_len;
    
    for (uint8_t i = 0; i < sensor_count && remaining > 1; i++) {
        if (!onewire_temp_valid(i)) {
            continue;
        }

        // , if not 1st
        if (current_len > 0 && remaining > 1) {
            *ptr++ = ',';
            remaining--;
            current_len++;
        }

        const uint8_t* addr = onewire_temp_address(i);
        int written = snprintf(sensor_name, sizeof(sensor_name),
                          "temp_%02x%02x%02x%02x%02x%02x%02x%02x",
                          addr[0], addr[1], addr[2], addr[3],
                          addr[4], addr[5], addr[6], addr[7]);
                          
        if (written < 0 || (size_t)written >= remaining) {
            break;
        }
        
        memcpy(ptr, sensor_name, written);
        ptr += written;
        remaining -= written;
        current_len += written;
    }
    *ptr = '\0';

    // pub node list
    snprintf(topic, sizeof(topic), "homie/%s/$nodes", node_id);
    publish_retained(client, topic, nodes_list);

    for (uint8_t i = 0; i < sensor_count; i++) {
        if (!onewire_temp_valid(i)) {
            continue;
        }

        const uint8_t* addr = onewire_temp_address(i);
        snprintf(sensor_name, sizeof(sensor_name),
                 "temp_%02x%02x%02x%02x%02x%02x%02x%02x",
                 addr[0], addr[1], addr[2], addr[3],
                 addr[4], addr[5], addr[6], addr[7]);

        snprintf(topic, sizeof(topic), "homie/%s/%s/$name", node_id, sensor_name);
        char display_name[48];
        snprintf(display_name, sizeof(display_name),
                 "Temperature Sensor %02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
                 addr[0], addr[1], addr[2], addr[3],
                 addr[4], addr[5], addr[6], addr[7]);
        publish_retained(client, topic, display_name);

        snprintf(topic, sizeof(topic), "homie/%s/%s/$properties", node_id, sensor_name);
        publish_retained(client, topic, "temperature,address,status,error_count");

        // temperature properties
        snprintf(topic, sizeof(topic), "homie/%s/%s/temperature/$name", node_id, sensor_name);
        publish_retained(client, topic, "Temperature");
        snprintf(topic, sizeof(topic), "homie/%s/%s/temperature/$datatype", node_id, sensor_name);
        publish_retained(client, topic, "float");
        snprintf(topic, sizeof(topic), "homie/%s/%s/temperature/$unit", node_id, sensor_name);
        publish_retained(client, topic, "°C");

        // address properties
        snprintf(topic, sizeof(topic), "homie/%s/%s/address/$name", node_id, sensor_name);
        publish_retained(client, topic, "ROM Address");
        snprintf(topic, sizeof(topic), "homie/%s/%s/address/$datatype", node_id, sensor_name);
        publish_retained(client, topic, "string");

        char addr_str[24];
        snprintf(addr_str, sizeof(addr_str),
                 "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
                 addr[0], addr[1], addr[2], addr[3],
                 addr[4], addr[5], addr[6], addr[7]);
        snprintf(topic, sizeof(topic), "homie/%s/%s/address", node_id, sensor_name);
        publish_retained(client, topic, addr_str);

        snprintf(topic, sizeof(topic), "homie/%s/%s/status/$name", node_id, sensor_name);
        publish_retained(client, topic, "Sensor Status");
        snprintf(topic, sizeof(topic), "homie/%s/%s/status/$datatype", node_id, sensor_name);
        publish_retained(client, topic, "enum");
        snprintf(topic, sizeof(topic), "homie/%s/%s/status/$format", node_id, sensor_name);
        publish_retained(client, topic, "valid,invalid");

        snprintf(topic, sizeof(topic), "homie/%s/%s/error_count/$name", node_id, sensor_name);
        publish_retained(client, topic, "Error Count");
        snprintf(topic, sizeof(topic), "homie/%s/%s/error_count/$datatype", node_id, sensor_name);
        publish_retained(client, topic, "integer");
    }
}

// MQTT
void onewire_temp_publish_values(MQTTClient* client, const char* node_id) {
  char topic[MAX_TOPIC_LENGTH];
  char value[8];
  uint8_t sensor_count = onewire_temp_count();

  for (uint8_t i = 0; i < sensor_count; i++) {
    bool is_valid = onewire_temp_valid(i);
    const uint8_t* addr = onewire_temp_address(i);

    char base_topic[MAX_TOPIC_LENGTH];
    snprintf(base_topic, sizeof(base_topic),
             "homie/%s/temp_%02x%02x%02x%02x%02x%02x%02x%02x",
             node_id, addr[0], addr[1], addr[2], addr[3], 
             addr[4], addr[5], addr[6], addr[7]);

    // publish status
    snprintf(topic, sizeof(topic), "%s/status", base_topic);
    publish_message(client, is_valid ? "valid" : "invalid", topic);

    // publish error cnt
    snprintf(topic, sizeof(topic), "%s/error_count", base_topic);
    snprintf(value, sizeof(value), "%u", ow_sys.sensors[i].error_count);
    publish_message(client, value, topic);

    snprintf(topic, sizeof(topic), "%s/temperature", base_topic);

    if (is_valid) {
      float temp = onewire_temp_get(i);
      if (temp == ONEWIRE_TEMP_INVALID) {
        publish_message(client, "0.00", topic);
      } else {
        // temp to str
        int16_t temp_fixed = (int16_t)(temp * 100);
        uint8_t neg = temp_fixed < 0;
        if (neg) temp_fixed = -temp_fixed;

        uint8_t idx = 0;
        if (neg) value[idx++] = '-';

        uint16_t whole = temp_fixed / 100;
        uint8_t decimal = temp_fixed % 100;

        if (whole >= 10) value[idx++] = '0' + (whole / 10);
        value[idx++] = '0' + (whole % 10);
        value[idx++] = '.';
        value[idx++] = '0' + (decimal / 10);
        value[idx++] = '0' + (decimal % 10);
        value[idx] = '\0';

        publish_message(client, value, topic);
      }
    } else {
      publish_message(client, "0.00", topic);
    }
  }
}