// can_handler.cpp - CAN FD Handler Implementation

#include <Arduino.h>
#include "can_handler.h"
#include "task_config.h"
#include "rtc_manager.h"
#include "psram_buffer.h"

ACAN2517FD canController(HSPI_CS_PIN, SPI, CAN_INT_PIN);

volatile bool canInitialized = false;
volatile uint32_t canRxCount = 0;
volatile uint32_t canTxCount = 0;
volatile uint32_t canErrorCount = 0;
volatile uint32_t canOverflowCount = 0;
static uint8_t currentCANMode = 0;

static CANFDMessage rxMessage;
static CANFDMessage txMessage;

bool initCAN() {
  Serial.println("Initializing CAN FD Controller (MCP2518FD)...");

  SPI.begin(HSPI_SCLK_PIN, HSPI_MISO_PIN, HSPI_MOSI_PIN, HSPI_CS_PIN);

  ACAN2517FDSettings settings(
    ACAN2517FDSettings::OSC_40MHz,
    CAN_DEFAULT_ARBITRATION_BAUDRATE,
    DataBitRateFactor::x4
  );

  settings.mISOCRCEnabled = true;
  settings.mDriverReceiveFIFOSize = MCP2518FD_RX_FIFO_SIZE;
  settings.mDriverTransmitFIFOSize = MCP2518FD_TX_FIFO_SIZE;
  
  switch (currentCANMode) {
    case 1:
      settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
      break;
    case 2:
    case 3:
      settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
      break;
    default:
      settings.mRequestedMode = ACAN2517FDSettings::NormalFD;
      break;
  }

  const uint32_t errorCode = canController.begin(settings, [] {
    onCANInterrupt();
  });

  if (errorCode == 0) {
    canInitialized = true;
    Serial.println("CAN FD initialized successfully!");
    Serial.printf("  Arbitration: %d bps\n", CAN_DEFAULT_ARBITRATION_BAUDRATE);
    Serial.printf("  Data: %d bps\n", CAN_DEFAULT_DATA_BAUDRATE);
    Serial.printf("  Mode: %d\n", currentCANMode);
    return true;
  } else {
    Serial.printf("CAN FD initialization failed! Error: 0x%X\n", errorCode);
    canInitialized = false;
    return false;
  }
}

bool setCANMode(uint8_t mode) {
  if (mode > 3) return false;
  
  currentCANMode = mode;
  
  if (canInitialized) {
    canController.end();
    canInitialized = false;
  }
  
  return initCAN();
}

uint8_t getCANMode() {
  return currentCANMode;
}

void IRAM_ATTR onCANInterrupt() {
  // Set flag or notify task - actual handling in task
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  
  // Notify CAN RX task
  if (canRxTaskHandle != NULL) {
    vTaskNotifyGiveFromISR(canRxTaskHandle, &xHigherPriorityTaskWoken);
  }
  
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void canRxTask(void *pvParameters) {
  Serial.println("CAN RX Task started on Core 0");
  
  uint32_t framesProcessed = 0;
  uint32_t lastStatusTime = 0;
  
  while (1) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    
    uint8_t batchCount = 0;
    while (canController.available() && batchCount < CAN_RX_BATCH_SIZE) {
      if (canController.receive(rxMessage)) {
        canRxCount++;
        batchCount++;
        
        CanFrame frame;
        frame.timestamp = getMicrosTimestamp();
        frame.id = rxMessage.id;
        frame.len = rxMessage.len;
        
        frame.flags = 0;
        if (rxMessage.type == CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH) {
          frame.flags |= 0x01;
        }
        if (rxMessage.type == CANFDMessage::CANFD_NO_BIT_RATE_SWITCH) {
          frame.flags |= 0x02;
        }
        
        memcpy(frame.data, rxMessage.data, rxMessage.len);
        
        if (xQueueSend(canRxQueue, &frame, 0) != pdTRUE) {
          if (!canFrameBuffer.push(frame)) {
            canErrorCount++;
          } else {
            canOverflowCount++;
          }
        }
        
        xQueueSend(graphQueue, &frame, 0);
      }
    }
    
    framesProcessed += batchCount;
    
    uint32_t now = millis();
    if (now - lastStatusTime > 5000) {
      Serial.printf("[CAN] RX: %d, Q: %d, PSRAM: %d/%d, Err: %d\n",
                    canRxCount, 
                    uxQueueMessagesWaiting(canRxQueue),
                    canFrameBuffer.available(),
                    canFrameBuffer.capacity(),
                    canErrorCount);
      lastStatusTime = now;
    }
  }
}

void canTxTask(void *pvParameters) {
  Serial.println("CAN TX Task started on Core 0");
  
  CanTxRequest request;
  
  while (1) {
    // Wait for transmit request
    if (xQueueReceive(canTxQueue, &request, portMAX_DELAY) == pdTRUE) {
      // Prepare message
      txMessage.id = request.id;
      txMessage.len = request.len;
      memcpy(txMessage.data, request.data, request.len);
      txMessage.type = CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH;
      
      // Handle repeat count
      uint32_t repeat = request.repeat_count;
      if (repeat == 0) {
        repeat = 1; // Send at least once
      }
      
      for (uint32_t i = 0; i < repeat; i++) {
        if (canController.tryToSend(txMessage)) {
          canTxCount++;
        } else {
          canErrorCount++;
          Serial.println("CAN TX failed: buffer full");
        }
        
        // Delay between repeats
        if (request.delay_ms > 0 && i < repeat - 1) {
          vTaskDelay(pdMS_TO_TICKS(request.delay_ms));
        }
      }
    }
  }
}

bool sendCANFrame(uint32_t id, uint8_t* data, uint8_t len, bool isFD) {
  if (!canInitialized) {
    return false;
  }
  
  CanTxRequest request;
  request.id = id;
  request.len = len;
  memcpy(request.data, data, len);
  request.delay_ms = 0;
  request.repeat_count = 1;
  
  return xQueueSend(canTxQueue, &request, pdMS_TO_TICKS(100)) == pdTRUE;
}

bool setCANBaudrate(uint32_t arbitrationBaud, uint32_t dataBaud) {
  return setCANBaudrateAndMode(arbitrationBaud, dataBaud, currentCANMode, true);
}

bool setCANBaudrateAndMode(uint32_t arbitrationBaud, uint32_t dataBaud, uint8_t mode, bool enableFD) {
  if (canInitialized) {
    canController.end();
    canInitialized = false;
  }

  currentCANMode = mode;

  DataBitRateFactor dataFactor = DataBitRateFactor::x1;
  
  if (enableFD) {
    uint32_t factor = dataBaud / arbitrationBaud;
    switch (factor) {
      case 1:  dataFactor = DataBitRateFactor::x1; break;
      case 2:  dataFactor = DataBitRateFactor::x2; break;
      case 4:  dataFactor = DataBitRateFactor::x4; break;
      case 5:  dataFactor = DataBitRateFactor::x5; break;
      case 6:  dataFactor = DataBitRateFactor::x6; break;
      case 7:  dataFactor = DataBitRateFactor::x7; break;
      case 8:  dataFactor = DataBitRateFactor::x8; break;
      case 10: dataFactor = DataBitRateFactor::x10; break;
      default: dataFactor = DataBitRateFactor::x4; break;
    }
  }

  ACAN2517FDSettings settings(
    ACAN2517FDSettings::OSC_40MHz,
    arbitrationBaud,
    dataFactor
  );

  settings.mISOCRCEnabled = enableFD;
  settings.mDriverReceiveFIFOSize = MCP2518FD_RX_FIFO_SIZE;
  settings.mDriverTransmitFIFOSize = MCP2518FD_TX_FIFO_SIZE;
  
  if (enableFD) {
    switch (mode) {
      case 1:
        settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
        break;
      case 2:
      case 3:
        settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
        break;
      default:
        settings.mRequestedMode = ACAN2517FDSettings::NormalFD;
        break;
    }
  } else {
    switch (mode) {
      case 1:
        settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
        break;
      case 2:
      case 3:
        settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
        break;
      default:
        settings.mRequestedMode = ACAN2517FDSettings::Normal20B;
        break;
    }
  }

  const uint32_t errorCode = canController.begin(settings, [] {
    onCANInterrupt();
  });

  canInitialized = (errorCode == 0);
  
  if (canInitialized) {
    Serial.printf("CAN configured: Arb=%d, Data=%d, FD=%s, Mode=%d\n",
                  arbitrationBaud, enableFD ? dataBaud : arbitrationBaud,
                  enableFD ? "enabled" : "disabled", mode);
  }
  
  return canInitialized;
}

void getCANStats(uint32_t& rx, uint32_t& tx, uint32_t& errors) {
  rx = canRxCount;
  tx = canTxCount;
  errors = canErrorCount;
}