// test_handler.cpp - Hardware Test Handler Implementation

#include "test_handler.h"
#include "can_handler.h"
#include "task_config.h"
#include "psram_buffer.h"

TestConfig testConfig;
TestResult testResult;
static uint32_t expectedSequence = 0;
static uint32_t receivedSequence = 0;
static TaskHandle_t testTaskHandle = NULL;

void initTestHandler() {
  testConfig.mode = TEST_MODE_IDLE;
  testConfig.frameCount = 1000;
  testConfig.intervalUs = 1000;
  testConfig.canId = 0x100;
  testConfig.dataLen = 8;
  testConfig.useFD = true;
  testConfig.running = false;
  
  memset(&testResult, 0, sizeof(TestResult));
}

uint32_t generateTestFrame(uint32_t sequence, CanFrame& frame) {
  frame.timestamp = micros();
  frame.id = testConfig.canId;
  frame.len = testConfig.useFD ? (testConfig.dataLen > 8 ? testConfig.dataLen : 8) : 8;
  frame.flags = testConfig.useFD ? 0x01 : 0x00;
  
  frame.data[0] = (sequence >> 24) & 0xFF;
  frame.data[1] = (sequence >> 16) & 0xFF;
  frame.data[2] = (sequence >> 8) & 0xFF;
  frame.data[3] = sequence & 0xFF;
  frame.data[4] = 0xDE;
  frame.data[5] = 0xAD;
  frame.data[6] = 0xBE;
  frame.data[7] = 0xEF;
  
  for (int i = 8; i < frame.len && i < 64; i++) {
    frame.data[i] = (uint8_t)(sequence + i);
  }
  
  return sequence;
}

bool validateTestFrame(const CanFrame& frame, uint32_t& expectedSeq) {
  if (frame.id != testConfig.canId) {
    return false;
  }
  
  uint32_t receivedSeq = ((uint32_t)frame.data[0] << 24) |
                         ((uint32_t)frame.data[1] << 16) |
                         ((uint32_t)frame.data[2] << 8) |
                         ((uint32_t)frame.data[3]);
  
  if (receivedSeq != expectedSeq) {
    uint32_t lost = receivedSeq - expectedSeq;
    testResult.framesLost += lost;
    expectedSeq = receivedSeq + 1;
    return true;
  }
  
  expectedSeq++;
  return true;
}

void testTxTask(void *pvParameters) {
  Serial.println("Test TX Task started");
  
  uint32_t sequence = 0;
  testResult.startTime = millis();
  testResult.framesSent = 0;
  testResult.framesReceived = 0;
  testResult.framesLost = 0;
  testResult.errors = 0;
  
  uint8_t originalMode = getCANMode();
  setCANMode(2);
  delay(100);
  
  expectedSequence = 0;
  
  while (testConfig.running && sequence < testConfig.frameCount) {
    CanFrame frame;
    generateTestFrame(sequence, frame);
    
    if (sendCANFrame(frame.id, frame.data, frame.len, testConfig.useFD)) {
      testResult.framesSent++;
      sequence++;
    } else {
      testResult.errors++;
    }
    
    if (testConfig.intervalUs > 0) {
      delayMicroseconds(testConfig.intervalUs);
    }
    
    while (canController.available()) {
      CANFDMessage rxMsg;
      if (canController.receive(rxMsg)) {
        CanFrame rxFrame;
        rxFrame.id = rxMsg.id;
        rxFrame.len = rxMsg.len;
        memcpy(rxFrame.data, rxMsg.data, rxMsg.len);
        
        if (rxFrame.id == testConfig.canId) {
          if (validateTestFrame(rxFrame, expectedSequence)) {
            testResult.framesReceived++;
          }
        }
      }
    }
  }
  
  delay(100);
  
  while (canController.available()) {
    CANFDMessage rxMsg;
    if (canController.receive(rxMsg)) {
      CanFrame rxFrame;
      rxFrame.id = rxMsg.id;
      rxFrame.len = rxMsg.len;
      memcpy(rxFrame.data, rxMsg.data, rxMsg.len);
      
      if (rxFrame.id == testConfig.canId) {
        if (validateTestFrame(rxFrame, expectedSequence)) {
          testResult.framesReceived++;
        }
      }
    }
  }
  
  testResult.endTime = millis();
  testResult.durationMs = testResult.endTime - testResult.startTime;
  
  if (testResult.durationMs > 0) {
    testResult.frameRate = (float)testResult.framesSent / ((float)testResult.durationMs / 1000.0f);
  }
  
  if (testResult.framesSent > 0) {
    testResult.lossRate = (float)testResult.framesLost / (float)testResult.framesSent * 100.0f;
  }
  
  testResult.passed = (testResult.framesLost == 0 && testResult.framesSent == testResult.framesReceived);
  
  setCANMode(originalMode);
  testConfig.running = false;
  testConfig.mode = TEST_MODE_IDLE;
  
  Serial.printf("Test completed: Sent=%d, Received=%d, Lost=%d, Rate=%.1f fps, Loss=%.2f%%\n",
                testResult.framesSent, testResult.framesReceived, testResult.framesLost,
                testResult.frameRate, testResult.lossRate);
  
  vTaskDelete(NULL);
  testTaskHandle = NULL;
}

bool startLoopbackTest(uint32_t frameCount, uint32_t intervalUs) {
  if (testConfig.running) {
    return false;
  }
  
  testConfig.mode = TEST_MODE_LOOPBACK;
  testConfig.frameCount = frameCount;
  testConfig.intervalUs = intervalUs;
  testConfig.canId = 0x100;
  testConfig.dataLen = 8;
  testConfig.useFD = false;
  testConfig.running = true;
  
  Serial.printf("Starting loopback test: %d frames, %d us interval\n", frameCount, intervalUs);
  
  xTaskCreatePinnedToCore(
    testTxTask,
    "TEST_TX",
    4096,
    NULL,
    6,
    &testTaskHandle,
    0
  );
  
  return true;
}

bool startStressTest(uint32_t frameCount, uint8_t dataLen, bool useFD) {
  if (testConfig.running) {
    return false;
  }
  
  testConfig.mode = TEST_MODE_STRESS;
  testConfig.frameCount = frameCount;
  testConfig.intervalUs = 0;
  testConfig.canId = 0x200;
  testConfig.dataLen = dataLen;
  testConfig.useFD = useFD;
  testConfig.running = true;
  
  Serial.printf("Starting stress test: %d frames, %d bytes, FD=%s\n", 
                frameCount, dataLen, useFD ? "true" : "false");
  
  xTaskCreatePinnedToCore(
    testTxTask,
    "TEST_TX",
    4096,
    NULL,
    6,
    &testTaskHandle,
    0
  );
  
  return true;
}

bool startSequenceTest(uint32_t frameCount, uint32_t canId) {
  if (testConfig.running) {
    return false;
  }
  
  testConfig.mode = TEST_MODE_SEQUENCE;
  testConfig.frameCount = frameCount;
  testConfig.intervalUs = 1000;
  testConfig.canId = canId;
  testConfig.dataLen = 64;
  testConfig.useFD = true;
  testConfig.running = true;
  
  Serial.printf("Starting sequence test: %d frames, ID=0x%X\n", frameCount, canId);
  
  xTaskCreatePinnedToCore(
    testTxTask,
    "TEST_TX",
    4096,
    NULL,
    6,
    &testTaskHandle,
    0
  );
  
  return true;
}

void stopTest() {
  if (testConfig.running) {
    testConfig.running = false;
    if (testTaskHandle != NULL) {
      vTaskDelay(pdMS_TO_TICKS(100));
    }
  }
}

void updateTest() {
}

bool isTestRunning() {
  return testConfig.running;
}

TestMode getTestMode() {
  return testConfig.mode;
}

TestResult getTestResult() {
  return testResult;
}

void getTestResultJSON(char* buffer, size_t bufferSize) {
  snprintf(buffer, bufferSize,
           "{\"running\":%s,\"mode\":%d,\"result\":{"
           "\"framesSent\":%d,\"framesReceived\":%d,\"framesLost\":%d,"
           "\"errors\":%d,\"durationMs\":%d,\"frameRate\":%.1f,"
           "\"lossRate\":%.2f,\"passed\":%s}}",
           testConfig.running ? "true" : "false",
           testConfig.mode,
           testResult.framesSent,
           testResult.framesReceived,
           testResult.framesLost,
           testResult.errors,
           testResult.durationMs,
           testResult.frameRate,
           testResult.lossRate,
           testResult.passed ? "true" : "false");
}