#include "sim800.h"
#include "drivers/pins.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "projdefs.h"
#include <stdio.h>
#define SIM800_UART UART_NUM_2

static QueueHandle_t uart_queue;

static char smsBufSend[512];
static uint8_t tcp_internal_buf[512];
static uint8_t *tcp_send_buf = NULL;
static volatile int tcp_send_len = 0;
static volatile bool tcp_waiting_prompt = false;

static volatile bool tcp_send_in_progress = false;

static char line_buf[512];
static volatile size_t line_pos = 0;

static const char *TAG = "SIM800";

static volatile bool sim_ready = false;
static volatile bool sms_ready = false;
static volatile bool sim_needs_pin = false;

static SemaphoreHandle_t sim800_done;
static bool sim800_waiting = false;
static bool sim800_result = false;
static modem_t modem;

static bool sms_waiting_prompt = false;

static SemaphoreHandle_t sim800_mutex;

static SemaphoreHandle_t sms_mutex;

void print_state() {
  switch (modem.state) {

  case MODEM_STATE_OFF:
    ESP_LOGI(TAG, "State is OFF");
    break;
  case MODEM_STATE_INIT:
    ESP_LOGI(TAG, "State is INIT");
    break;
  case MODEM_STATE_READY:
    ESP_LOGI(TAG, "State is Ready");
    break;
  case MODEM_STATE_REGISTERING:
    ESP_LOGI(TAG, "State is Registering");
    break;
  case MODEM_STATE_REGISTERED:
    ESP_LOGI(TAG, "State is Registered");
    break;
  case MODEM_STATE_SMS_READY:
    ESP_LOGI(TAG, "State is SMS Ready");
    break;
  case MODEM_STATE_CALL_ACTIVE:
    ESP_LOGI(TAG, "State is Call active");
    break;
  case MODEM_STATE_GPRS_CONNECTING:
    ESP_LOGI(TAG, "State is GPRS Connecting");
    break;
  case MODEM_STATE_GPRS_UP:
    ESP_LOGI(TAG, "State is GPRS UP");
    break;
  case MODEM_STATE_TCP_CONNECTING:
    ESP_LOGI(TAG, "State is TCP Connecting");
    break;
  case MODEM_STATE_TCP_CONNECTED:
    ESP_LOGI(TAG, "State is TCP Connected");
    break;
  case MODEM_STATE_ERROR:
    ESP_LOGI(TAG, "State is Error");
    break;
  }
}

void sim800_uart_init(void) {
  uart_config_t uart_config = {
      .baud_rate = 9600,
      .data_bits = UART_DATA_8_BITS,
      .parity = UART_PARITY_DISABLE,
      .stop_bits = UART_STOP_BITS_1,
      .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
      .source_clk = UART_SCLK_APB,
  };

  uart_driver_install(SIM800_UART, 2048, 2048, 20, &uart_queue, 0);

  ESP_LOGI(TAG, "UART queue: %p", uart_queue);

  uart_param_config(SIM800_UART, &uart_config);

  uart_set_pin(SIM800_UART, HEADER_UART_TX, HEADER_UART_RX, UART_PIN_NO_CHANGE,
               UART_PIN_NO_CHANGE);
}

void modem_update_flags(void) {
  modem.in_call = (modem.state == MODEM_STATE_CALL_ACTIVE);

  modem.gprs_active = (modem.state == MODEM_STATE_GPRS_UP);
}

void modem_set_state(modem_state_t new_state) {
  xSemaphoreTake(modem.state_mutex, portMAX_DELAY);

  if (modem.state == new_state) {
    xSemaphoreGive(modem.state_mutex);
    return;
  }

  ESP_LOGI(TAG, "STATE: %d -> %d", modem.state, new_state);

  modem.state = new_state;

  print_state();

  xSemaphoreGive(modem.state_mutex);

  modem_update_flags();
}

void modem_handle_event(modem_event_t ev) {
  switch (modem.state) {

  // -----------------------
  case MODEM_STATE_INIT:
    if (ev == EV_MODEM_OK) {
      modem_set_state(MODEM_STATE_READY);
    }
    break;

  // -----------------------
  case MODEM_STATE_READY:
    if (ev == EV_NETWORK_REG_OK) {
      modem_set_state(MODEM_STATE_REGISTERED);
    }

    if (ev == EV_INCOMING_CALL) {
      bool was_gprs = modem.gprs_active;

      modem_set_state(MODEM_STATE_CALL_ACTIVE);

      if (was_gprs) {
        sim800_command("AT+CIPSHUT", 5000);
        modem_handle_event(EV_GPRS_DOWN);
      }
    }

    if (ev == EV_SMS_RECEIVED) {
      // stay in same state (SMS is async service)
    }
    break;

  // -----------------------
  case MODEM_STATE_REGISTERED:
    if (ev == EV_GPRS_UP) {
      modem_set_state(MODEM_STATE_GPRS_UP);
    }

    if (ev == EV_INCOMING_CALL) {
      modem_set_state(MODEM_STATE_CALL_ACTIVE);
    }

    if (ev == EV_NETWORK_LOST) {
      modem_set_state(MODEM_STATE_REGISTERING);
    }
    break;

  // -----------------------
  case MODEM_STATE_GPRS_UP:
    if (ev == EV_GPRS_DOWN) {
      modem.tcp_connected = false;
      modem_set_state(MODEM_STATE_SMS_READY);
    }

    if (ev == EV_INCOMING_CALL) {
      // SIM800 limitation: calls break data
      modem_set_state(MODEM_STATE_CALL_ACTIVE);
    }
    break;

  // -----------------------
  case MODEM_STATE_CALL_ACTIVE:
    if (ev == EV_CALL_ENDED) {
      modem_set_state(MODEM_STATE_SMS_READY);
    }
    break;

  default:
    break;
  }
}

bool modem_start_gprs(void) {
  if (modem.state != MODEM_STATE_REGISTERED &&
      modem.state != MODEM_STATE_SMS_READY)
    return false;

  modem_set_state(MODEM_STATE_GPRS_CONNECTING);

  // Always reset stack first
  sim800_command("AT+CIPSHUT", 10000);

  sim800_command("AT+CIPMUX=0", 10000);

  // Ensure network attach (retry is IMPORTANT)
  for (int i = 0; i < 5; i++) {
    if (sim800_command("AT+CGATT=1", 3000))
      break;
    vTaskDelay(pdMS_TO_TICKS(100));
  }

  if (!sim800_command("AT+CGATT?", 2000)) {
    ESP_LOGW(TAG, "CGATT check failed");
  }

  if (!sim800_command("AT+CSTT=\"internet\"", 5000))
    return false;

  if (!sim800_command("AT+CIICR", 15000))
    return false;

  vTaskDelay(pdMS_TO_TICKS(100));

  sim800_command("AT+CIFSR", 100);

  if (!sim800_command("AT+CIPHEAD=1", 15000))
    return false;

  modem_handle_event(EV_GPRS_UP);
  modem_set_state(MODEM_STATE_GPRS_UP);
  ESP_LOGI(TAG, "STARTED GPRS");
  return true;
}

void sim800_csq_task(void *arg) {
  while (1) {
    /*
    if (modem.state == MODEM_STATE_TCP_CONNECTED ||
        modem.state == MODEM_STATE_GPRS_UP) {
      sim800_command("AT+CSQ", 2000);
    }
      */
    if (modem.need_gprs_restart && modem.network_registered) {
      modem.need_gprs_restart = false;
      ESP_LOGI(TAG, "STARTING GPRS");
      print_state();
      modem_start_gprs();
      print_state();
      vTaskDelay(pdMS_TO_TICKS(100));
    }

    print_state();
    if (modem.state == MODEM_STATE_GPRS_UP) {
      sim800_tcp_connect("brn.systems", 6969);
      if (modem.state != MODEM_STATE_TCP_CONNECTED) {
        vTaskDelay(pdMS_TO_TICKS(1000));
      }
    }

    vTaskDelay(pdMS_TO_TICKS(5000));
  }
}

void sim800_task(void *arg)
{
    uart_event_t event;
    uint8_t *data = malloc(1024);

    char lineBuf[128];
    int linePos = 0;

    int ipdRemaining = 0;
    int ipdActive = 0;

    while (1)
    {
        if (xQueueReceive(uart_queue, &event, portMAX_DELAY))
        {
            if (event.type == UART_DATA)
            {
                int len = uart_read_bytes(SIM800_UART, data, event.size, pdMS_TO_TICKS(100));

                for (int i = 0; i < len; i++)
                {
                    char c = data[i];

                    /* =========================
                       RAW TCP MODE (NO PARSING)
                       ========================= */
                    if (ipdActive)
                    {
                        tcp_on_byte(c);

                        if (--ipdRemaining <= 0)
                        {
                            ipdActive = 0;
                            ESP_LOGI(TAG, "TCP RX END");
                        }

                        continue;
                    }

                    /* =========================
                       CONTROL LINE BUILDING
                       ========================= */
                    if (c == '\r')
                        continue;

                    if (c == '\n')
                    {
                        if (linePos > 0)
                        {
                            lineBuf[linePos] = 0;

                            /* =========================
                               DETECT +IPD HEADER
                               ========================= */
                            if (strncmp(lineBuf, "+IPD,", 5) == 0)
                            {
                                int len = 0;
                                char *p = strchr(lineBuf, ':');

                                if (p)
                                {
                                    *p = 0;
                                    sscanf(lineBuf, "+IPD,%d", &len);

                                    ipdRemaining = len;
                                    ipdActive = 1;

                                    ESP_LOGI(TAG, "TCP RX START len=%d", len);

                                    // if data after ':' already exists in same packet
                                    p++;
                                    while (*p && ipdRemaining > 0)
                                    {
                                        tcp_on_byte(*p++);
                                        ipdRemaining--;
                                    }

                                    if (ipdRemaining <= 0)
                                    {
                                        ipdActive = 0;
                                        ESP_LOGI(TAG, "TCP RX END (inline)");
                                    }
                                }
                            }
                            else
                            {
                                sim800_handle_line(lineBuf);
                            }

                            linePos = 0;
                        }
                        continue;
                    }

                    /* =========================
                       BUILD LINE BUFFER (CTRL)
                       ========================= */
                    if (linePos < sizeof(lineBuf) - 1)
                    {
                        lineBuf[linePos++] = c;
                    }
                }
            }

            if (event.type == UART_FIFO_OVF)
            {
                ESP_LOGW(TAG, "FIFO overflow");
                uart_flush_input(SIM800_UART);
                xQueueReset(uart_queue);
            }
        }
    }

    free(data);
}

void sim800_handle_line(char *line) {
  ESP_LOGI(TAG, "LINE: [%s]", line);

  // -------------------------
  // COMMAND RESPONSE HANDLER
  // -------------------------
  if (sim800_waiting) {

    if (strcmp(line, "OK") == 0) {
      ESP_LOGI(TAG, "Command successful");
      sim800_result = true;
      sim800_waiting = false;
      xSemaphoreGive(sim800_done);
      return;
    }

    if (strcmp(line, "ERROR") == 0) {
      sim800_result = false;
      sim800_waiting = false;
      tcp_send_in_progress = false;
      xSemaphoreGive(sim800_done);
      return;
    }
    if (strstr(line, "ERROR") != 0) {
      sim800_result = false;
      sim800_waiting = false;
      tcp_send_in_progress = false;
      xSemaphoreGive(sim800_done);
      return;
    }
  }

  // -------------------------
  // CALL EVENTS
  // -------------------------
  if (strcmp(line, "RING") == 0) {
    ESP_LOGI(TAG, "Incoming call");
    modem.in_call = true;
    if (modem.state == MODEM_STATE_GPRS_UP) {
      sim800_command("AT+CIPSHUT", 10000);
      modem_handle_event(EV_GPRS_DOWN);
    }
    modem_handle_event(EV_INCOMING_CALL);
    return;
  }

  if (strncmp(line, "NO CARRIER", 11) == 0) {
    ESP_LOGI(TAG, "Call ended");
    modem.in_call = false;
    modem_handle_event(EV_CALL_ENDED);

    if (modem.network_registered) {
      modem.need_gprs_restart = true;
    }
    return;
  }

  if (strncmp(line, "BUSY", 4) == 0) {
    modem.in_call = false;
    modem_handle_event(EV_CALL_ENDED);
    if (modem.network_registered) {
      modem.need_gprs_restart = true;
    }
    return;
  }

  // -------------------------
  // SMS EVENTS
  // -------------------------
  if (strncmp(line, "+CMTI:", 6) == 0) {
    int index = -1;
    sscanf(line, "+CMTI: \"SM\",%d", &index);

    ESP_LOGI(TAG, "New SMS at index %d", index);

    char cmd[32];
    snprintf(cmd, sizeof(cmd), "AT+CMGR=%d", index);
    sim800_command(cmd, 5000);

    modem_handle_event(EV_SMS_RECEIVED);

    return;
  }

  if (strncmp(line, "+CMGR:", 6) == 0) {
    ESP_LOGI(TAG, "SMS header received");
    return;
  }

  // -------------------------
  // SIGNAL QUALITY
  // -------------------------
  if (strncmp(line, "+CSQ:", 5) == 0) {
    int rssi, ber;

    if (sscanf(line, "+CSQ: %d,%d", &rssi, &ber) == 2) {

      modem.signal = rssi;
      modem.last_rssi = rssi;
      modem.last_ber = ber;

      ESP_LOGI(TAG, "CSQ RSSI=%d BER=%d", rssi, ber);

      // Optional: trigger behavior
      if (rssi == 99) {
        ESP_LOGW(TAG, "No signal");
      } else if (rssi < 10) {
        ESP_LOGW(TAG, "Weak signal");
      }
    }
    return;
  }

  // -------------------------
  // NETWORK REGISTRATION
  // -------------------------
  if (strncmp(line, "+CREG:", 6) == 0) {
    ESP_LOGI(TAG, "Network registration: %s", line);
    int n, stat;
    if (strlen(line) > 10) {
      sscanf(line, "+CREG: %d,%d", &n, &stat);
      ESP_LOGI(TAG, "Network registration data: %d,%d", n, stat);

      if (stat == 1 || stat == 5) {
        modem.network_registered = 1;
        ESP_LOGI(TAG, "REGISTERED");
        modem_handle_event(EV_NETWORK_REG_OK);
      } else {
        modem.network_registered = 0;
        modem_handle_event(EV_NETWORK_LOST);
      }
    } else {
      sscanf(line, "+CREG: %d", &stat);
      ESP_LOGI(TAG, "Network registration data: %d,%d", stat);

      if (stat == 1 || stat == 5) {
        modem.network_registered = 1;
        ESP_LOGI(TAG, "REGISTERED");
        modem_handle_event(EV_NETWORK_REG_OK);
      } else {
        modem.network_registered = 0;
        modem_handle_event(EV_NETWORK_LOST);
      }
    }
    return;
  }

  // -------------------------
  // GPRS / DATA EVENTS
  // -------------------------
  if (strncmp(line, "CONNECT OK", 10) == 0) {
    ESP_LOGI(TAG, "TCP connected");
    modem_set_state(MODEM_STATE_TCP_CONNECTED);

    modem.tcp_connected = true;

    modem_set_state(MODEM_STATE_TCP_CONNECTED);

    return;
  }

  if (strstr(line, "CONNECT FAIL")) {
    modem.tcp_connected = false;
    modem_set_state(MODEM_STATE_GPRS_UP);
  }

  if (strstr(line, "+CPIN: READY")) {
    sim_ready = true;
    modem_handle_event(EV_MODEM_OK);
    ESP_LOGI(TAG, "SIM ready");
    return;
  }

  if (strstr(line, "+CPIN: SIM PIN")) {
    sim_needs_pin = true;
    ESP_LOGI(TAG, "SIM needs pin");
    return;
  }

  if (strstr(line, "CALL Ready")) {
    modem.network_registered = 1;
    ESP_LOGI(TAG, "SMS subsystem ready");
    return;
  }

  if (strstr(line, "SMS Ready")) {
    sms_ready = true;
    modem.network_registered = 1;
    ESP_LOGI(TAG, "SMS subsystem ready");
    return;
  }

  if (strncmp(line, "SEND OK", 7) == 0) {
    ESP_LOGI(TAG, "Data sent");
    tcp_send_in_progress = false;
    return;
  }

  if (strncmp(line, "CLOSED", 6) == 0) {
    tcp_send_in_progress = false;
    modem.tcp_connected = false;
    modem_set_state(MODEM_STATE_GPRS_UP);
    return;
  }

  /*
  if (strncmp(line, "+IPD,", 5) == 0) {

    ESP_LOGI(TAG, "TCP DATA: %s", line);

    int len = 0;
    sscanf(line, "+IPD,%d", &len);
    // format: +IPD,len:data
    char *p = strchr(line, ':');
    if (p) {
      p++;

      ESP_LOGI(TAG, "RX TCP payload: %s", p);

      // TODO: forward to your app layer
      tcp_on_data(p, len);
    }

    return;
  }
  

  if (strncmp(line, "+RECEIVE", 8) == 0) {

    char *payload = strchr(line, ':');

    int pipe = 0;
    if (payload) {
      payload++; // skip ':'
      int len = 0;
      sscanf(line, "+RECEIVE,%d,%d", &pipe, &len);

      ESP_LOGI(TAG, "TCP RX (%d): %.*s", pipe, len, payload);

      tcp_on_data(payload, len);
    }

    return;
  }
    */
}

void sim800_parse(char *data) {
  while (*data) {

    char c = *data++;

    putchar(c);

    if (c == '\r')
      continue;

    if (line_buf[0] == '>') {

      if (sms_waiting_prompt) {
        sms_waiting_prompt = false;

        uart_write_bytes(SIM800_UART, smsBufSend, strlen(smsBufSend));
        uint8_t ctrlz = 0x1A;
        uart_write_bytes(SIM800_UART, &ctrlz, 1);
      }

      if (tcp_waiting_prompt) {
        ESP_LOGI(TAG, "Got TCP >");
        tcp_waiting_prompt = false;

        uart_write_bytes(SIM800_UART, tcp_send_buf, tcp_send_len);

        uint8_t ctrlz = 0x1A;
        uart_write_bytes(SIM800_UART, &ctrlz, 1);
        tcp_send_in_progress = false;
      }

      continue;
    }

    if (c == '\n') {

      if (line_pos > 0) {

        line_buf[line_pos] = 0;

        sim800_handle_line(line_buf);

        line_pos = 0;
      }

      continue;
    }

    if (line_pos < sizeof(line_buf) - 1) {
      line_buf[line_pos++] = c;
    }
  }
}

void sim800_send(const char *cmd) {
  ESP_LOGI(TAG, "TX: %s", cmd);

  uart_write_bytes(SIM800_UART, cmd, strlen(cmd));

  uart_write_bytes(SIM800_UART, "\r\n", 2);
}

bool sim800_command(const char *cmd, uint32_t timeout_ms) {
  xSemaphoreTake(sim800_mutex, portMAX_DELAY);

  sim800_waiting = true;
  sim800_result = false;

  sim800_send(cmd);

  bool ok = xSemaphoreTake(sim800_done, pdMS_TO_TICKS(timeout_ms));

  if (!ok) {
    sim800_waiting = false; // important cleanup
    ESP_LOGW(TAG, "Timeout");

    xSemaphoreGive(sim800_mutex);
    return false;
  }

  bool result = sim800_result;

  sim800_waiting = false;
  xSemaphoreGive(sim800_mutex);

  return result;
}

void init_modem(void *arg) {
  modem.state_mutex = xSemaphoreCreateMutex();
  modem.state = MODEM_STATE_INIT;
  modem.network_registered = 0;
  modem.in_call = false;
  modem.gprs_active = false;
  modem.need_gprs_restart = true;
  sim800_done = xSemaphoreCreateBinary();
  sim800_mutex = xSemaphoreCreateMutex();
  sms_mutex = xSemaphoreCreateMutex();
  xTaskCreate(sim800_task, "sim800_task", 4096, NULL, 10, NULL);
  sim800_command("AT+CFUN=1,1", 1);
  while (1) {
    if (sim800_command("AT", 1000)) {
      ESP_LOGI(TAG, "Modem alive");
      sim800_command("ATE0", 100);
      sim800_command("AT+IPR=115200", 100);
      uart_set_baudrate(SIM800_UART, 115200);
      vTaskDelay(pdMS_TO_TICKS(50));
      while (!sim_needs_pin) {
        vTaskDelay(pdMS_TO_TICKS(100));
      }
      sim800_command("AT+CPIN=\"1577\"", 5000); // change to used sim, this is a test pin
      while (!sim_ready) {
        vTaskDelay(pdMS_TO_TICKS(100));
      }
      /*
      while (!sms_ready) {
        vTaskDelay(pdMS_TO_TICKS(100));
      }
        */
      sim800_command("AT+CREG=1", 3000);
      while (!modem.network_registered) {
        sim800_command("AT+CREG?", 3000);
        vTaskDelay(pdMS_TO_TICKS(100));
      }
      sim800_command("AT+CMEE=2", 100);
      sim800_command("AT+CMGF=1", 3000);
      sim800_command("AT+CNMI=2,1,0,0,0", 3000);
      xTaskCreate(sim800_csq_task, "csq_task", 3072, NULL, 5, NULL);
      break;
    } else {
      ESP_LOGE(TAG, "No response");
    }
  }
  while (1) {
    vTaskDelay(pdTICKS_TO_MS(1000));
  }
}

bool sim800_send_sms(const char *number, const char *msg) {
  xSemaphoreTake(sms_mutex, portMAX_DELAY);

  strcpy(smsBufSend, msg);

  char cmd[64];
  snprintf(cmd, sizeof(cmd), "AT+CMGS=\"%s\"", number);

  sms_waiting_prompt = true;
  sim800_send(cmd);

  return true;
}

bool sim800_call(const char *number) {
  if (modem.gprs_active) {
    sim800_command("AT+CIPSHUT", 5000);
    modem_handle_event(EV_GPRS_DOWN);
  }

  char cmd[64];
  snprintf(cmd, sizeof(cmd), "ATD%s;", number);

  return sim800_command(cmd, 15000);
}
bool sim800_hangup(void) { return sim800_command("ATH", 3000); }

bool sim800_answer(void) { return sim800_command("ATA", 5000); }

bool sim800_tcp_connect(const char *host, int port) {
  if (modem.state != MODEM_STATE_GPRS_UP)
    return false;

  snprintf(modem.tcp_host, sizeof(modem.tcp_host), "%s", host);
  modem.tcp_port = port;

  modem_set_state(MODEM_STATE_TCP_CONNECTING);

  char cmd[128];
  snprintf(cmd, sizeof(cmd), "AT+CIPSTART=\"TCP\",\"%s\",\"%d\"", host, port);

  if (!sim800_command(cmd, 15000)) {
    modem_set_state(MODEM_STATE_GPRS_UP);
    return false;
  }

  return true;
}

bool sim800_tcp_send(const void *data, int len) {
  if (modem.state != MODEM_STATE_TCP_CONNECTED)
    return false;

  if (tcp_send_in_progress)
    return false;
  tcp_send_in_progress = true;

  xSemaphoreTake(sim800_mutex, portMAX_DELAY);

  memcpy(tcp_internal_buf, data, len);
  tcp_send_buf = tcp_internal_buf;
  tcp_send_len = len;
  tcp_waiting_prompt = true;

  char cmd[32];
  snprintf(cmd, sizeof(cmd), "AT+CIPSEND=%d", len);
  sim800_send(cmd);

  // actual sending happens when '>' arrives in parser

  xSemaphoreGive(sim800_mutex);
  return true;
}

bool sim800_tcp_close(void) {
  if (!modem.tcp_connected)
    return true;

  bool ok = sim800_command("AT+CIPCLOSE", 5000);

  modem.tcp_connected = false;

  modem_set_state(MODEM_STATE_GPRS_UP);

  return ok;
}