#include "CanDataHandler.h"
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "CLSAddress.h"
#include "fatfs.h"
#include "FirmwareUpdate.h"
#include "UsbDataHandler.h"

// Memory for the task
StaticTask_t CLS_FW_Task_cb;
uint32_t CLS_FW_Task_stk[512];
// Attributes for the task
osThreadId_t CLS_FW_Task_id;
const osThreadAttr_t CLS_FW_Task_attr = {
  .name = "CLS_FW_Task",
  .attr_bits = 0U,
  .cb_mem = &CLS_FW_Task_cb,
  .cb_size = sizeof(CLS_FW_Task_cb),
  .stack_mem = CLS_FW_Task_stk,
  .stack_size = sizeof(CLS_FW_Task_stk),
  .priority = osPriorityNormal,
  .reserved = 0U
};

osMutexId_t CLS_FW_Task_FileLock = NULL;
static FIL fw_file;
static FwFrame frame = {0};



// wait_for_start_callback to get notified when the slave is ready
void wait_for_start_callback(uint16_t canid, uint8_t * data, uint8_t size) {
    osThreadFlagsSet(CLS_FW_Task_id,  TASK_FLAG_WAIT);
}

void wait_for_start(FirmwareUpdateArgs * configuration) {

    // setup wait_for_start_callback to get notified when the slave is ready
    uint16_t ready_canid = GENERATE_CLS_ADDRESS(CLS_CODE_FIMWARE, configuration->device, CLS_CH_FW_MISO);
    // wait_for_start_callback // MessageCode::Firmware(FirmwareChannel::SlaveOutMasterIn);
    CanData_regEventMsg(ready_canid, wait_for_start_callback);

    // setup the command to send the slave into booloader mode
    uint16_t target_canid = GENERATE_CLS_ADDRESS(CLS_CODE_FIMWARE, configuration->device, CLS_CH_FW_BOOTCALL);
    uint8_t bootloader_call[8] = {0xab, 0xab, 0xab, 0xab, 0xab, 0xab, 0xab, 0xab};


    // wait for the slave to be ready to update:
    uint32_t counter = 0;
    do {
        // send bootloader command
        CLS_BSP_TxHeaderType header = CREATE_BSP_CAN_HEADER(target_canid,CLS_BSP_DLC_BYTES_8);
        CLS_BSP_CAN_AddMessageToSend(&header,bootloader_call); 
        counter++;

        // after some time abort if we dont get a connection
        if(counter > BOOTLOADER_CALL_MAX_TRIES) {
            // TODO: Logging
            CanData_removeEvent(ready_canid);
            f_close(&fw_file);
            osThreadExit();
        }

    // wait for the slave
    } while(osThreadFlagsWait(TASK_FLAG_WAIT,osFlagsWaitAny,BOOTLOADER_CALL_RETRY_TIME) == (uint32_t)osErrorTimeout);
    
    //remove the wait_for_start_callback again
    CanData_removeEvent(ready_canid);

    // reset the fw_file
    f_lseek(&fw_file,0);
    frame.counter = 0;
}



void running_callback_error(uint16_t canid, uint8_t * data, uint8_t size) {
    osMutexAcquire(CLS_FW_Task_FileLock, osWaitForever);
    f_lseek(&fw_file,0);
    frame.counter = 0;
    osMutexRelease(CLS_FW_Task_FileLock);
    osThreadFlagsSet(CLS_FW_Task_id,  TASK_FLAG_RUNNING);
}

void running_callback_ack(uint16_t canid, uint8_t * data, uint8_t size) { 
    osThreadFlagsSet(CLS_FW_Task_id,  TASK_FLAG_RUNNING);
}


void running(FirmwareUpdateArgs * configuration) {
    uint16_t error_canid = GENERATE_CLS_ADDRESS(CLS_CODE_FIMWARE, configuration->device, CLS_CH_FW_MISO);
    uint16_t ack_canid = GENERATE_CLS_ADDRESS(CLS_CODE_FIMWARE, configuration->device, CLS_CH_FW_SLAVE_FEEDBACK);
    uint16_t target_canid = GENERATE_CLS_ADDRESS(CLS_CODE_FIMWARE, configuration->device, CLS_CH_FW_MOSI);
    CLS_BSP_TxHeaderType header = CREATE_BSP_CAN_HEADER(target_canid,CLS_BSP_DLC_BYTES_8);

    CanData_regEventMsg(error_canid,running_callback_error);
    CanData_regEventMsg(ack_canid, running_callback_ack);

    unsigned int read_bytes = 0;

    while (true)
    {
        
        // read fw file and and send data
        osMutexAcquire(CLS_FW_Task_FileLock, osWaitForever);
        f_read(&fw_file, &frame.data, sizeof(frame.data), &read_bytes);
        CLS_BSP_CAN_AddMessageToSend(&header, (uint8_t*)&frame);
        frame.counter++;
        osMutexRelease(CLS_FW_Task_FileLock);

        // transport window response
        if(frame.counter % UPDATE_ACK_WINDOW == 0) {
            osThreadFlagsWait(TASK_FLAG_RUNNING, osFlagsWaitAny, osWaitForever);
        }

        // end of file
        if(read_bytes != sizeof(frame.data)) { 
            break;
        }

    }


    uint64_t done = UPDATE_DONE_MSG;
    CLS_BSP_CAN_AddMessageToSend(&header,(uint8_t*)&done); 

    CanData_removeEvent(error_canid);
    CanData_removeEvent(ack_canid);
}


#define MAX_FW_UPDATES 16
osMessageQueueId_t fwUpdateQueue;

void CLSFirmwareUpdateTask_func(void *argument);

void FirmwareUpdateTask_start(FirmwareUpdateArgs args) {
  // Task functionality here
    if(CLS_FW_Task_FileLock == NULL) {
        CLS_FW_Task_FileLock = osMutexNew(NULL);
    }

        // Initialize the queue if it hasn't been initialized yet
    if(fwUpdateQueue == NULL) {
        fwUpdateQueue = osMessageQueueNew(MAX_FW_UPDATES, sizeof(FirmwareUpdateArgs), NULL);
    }

    if(CLS_FW_Task_FileLock == NULL) { 
        // error cant create mutex
        return;
    }   

    osMessageQueuePut(fwUpdateQueue, &args, 0, 0);


  // check CLS_FW_Task_id is null or stopped
  // only one Task can runn at once
   if(CLS_FW_Task_id == NULL || osThreadGetState(CLS_FW_Task_id) == osThreadTerminated) {
    CLS_FW_Task_id = osThreadNew(CLSFirmwareUpdateTask_func, NULL, &CLS_FW_Task_attr);
   }
}

void CLSFirmwareUpdateTask_func(void *argument) {
    FirmwareUpdateArgs args;
    osStatus_t status;

    while(1) {
        // Wait for a firmware update request
        status = osMessageQueueGet(fwUpdateQueue, &args, NULL, 100);

        // If the queue is empty, terminate the task
        if (status != osOK) {
            osThreadExit();
        }

        char * filename = args.name;
        if(f_open(&fw_file, filename, FA_READ) != FR_OK) {
            // TODO: Logging
            continue;
        }
    
        wait_for_start(&args);

        running(&args);

        //FirmwareUpdateDone done = {
        //    .device_id = args.device,
        //};
        //UsbDataPacketSendMessage(UsbPackageType_FIRMWAREUPDATEDONE, &buffer,FirmwareUpdateDone_fields,&done);
        
        f_close(&fw_file);
    }
}