#include "stdbool.h"
#include "stdint.h"

#include "api_debug.h"
#include "api_event.h"
#include "api_hal_pm.h"
#include "api_os.h"

#include "api_fs.h"
#include "api_hal_flash.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define MAIN_TASK_STACK_SIZE (2048 * 2)
#define MAIN_TASK_PRIORITY   0
#define MAIN_TASK_NAME       "Main Test Task"

#define SECOND_TASK_STACK_SIZE (2048 * 2)
#define SECOND_TASK_PRIORITY   1
#define SECOND_TASK_NAME       "Second Test Task"

static HANDLE mainTaskHandle   = NULL;
static HANDLE secondTaskHandle = NULL;

void EventDispatch(API_Event_t* pEvent) {
  switch (pEvent->id) {
    default: break;
  }
}

void DumpBlock(uint8_t* ptr, size_t len) {
  Trace(1, "%08x", (unsigned)ptr);
  size_t size = len;
  char buf[256];
  memset(buf, 0, sizeof(buf));
  while (size) {
    for (int i = 0; i < 32; i += 4) { sprintf(&buf[(i / 4) * 9], "%02x%02x%02x%02x ", ptr[i], ptr[i + 1], ptr[i + 2], ptr[i + 3]); }
    Trace(1, buf);
    ptr += 32;
    size -= 32;
  }
}

void SaveBlock(uint8_t* ptr, size_t sz, char* file) {
  Trace(1, "Writing %dk from %08x to %s", sz / 1024, (uint32_t)ptr, file);
  int32_t fd = API_FS_Open(file, FS_O_RDWR | FS_O_CREAT | FS_O_TRUNC, 0);
  if (fd < 0) {
    Trace(1, "Unable to write %s", file);
    return;
  }
  int32_t len = API_FS_Write(fd, (uint8_t*)ptr, sz);
  if (len < 0) {
    Trace(1, "Unable to save %d", len);
    return;
  }
  API_FS_Flush(fd);
  API_FS_Close(fd);
  Trace(1, "Done");
}

size_t flashblock = 4 * 1024;

void WriteFlash(char* file, uint32_t addr, int32_t sz) {
  Trace(1, "Storing %dk at %08x from %s", sz / 1024, addr, file);

  int32_t fd = API_FS_Open(file, FS_O_RDONLY, 0);
  if (fd < 0) {
    Trace(1, "Unable to read %s", file);
    return;
  }

  int32_t len = API_FS_GetFileSize(fd);
  if (len != sz) {
    Trace(1, "Source size wrong %d vs %d", len, sz);
    return;
  }

  // Do in 64k chunks
  uint8_t* ptr = malloc(flashblock);

  if (!ptr) {
    Trace(1, "Unable to malloc %d", flashblock);
    return;
  }

  Trace(1, "Burn");

  int32_t chunk;
  int     i = 0;
  for (chunk = 0; chunk < sz; chunk += flashblock) {
    size_t chk = flashblock;
    if (sz < chk) chk = sz;
    else if (sz - chunk < chk)
      chk = sz - chunk;

    API_FS_Read(fd, ptr, chk);
    Trace(1, "%d", i++);

    if (!hal_SpiFlashErase(addr + chunk, chk)) {
      Trace(1, "Unable to erase");
      return;
    }
    if (!hal_SpiFlashWrite(addr + chunk, ptr, chk)) {
      Trace(1, "Unable to write");
      break;
    }
    OS_Sleep(10);
  }
  free(ptr);
  Trace(1, "Burnt %d chunks of %d", (chunk / flashblock), flashblock);
}

void SecondTask(void* pData) {
//  PM_SetSysMinFreq(624000000); //?does this help?

  Trace(1, "Start");
  OS_Sleep(5000);

  hal_SpiFlashUnlock(); // or unlock to write??
  hal_SpiFlashOpen();

  uint32_t id = hal_SpiFlashGetID();

  uint8_t mf = id & 0xff;
  uint8_t tp = (id >> 8) & 0xff;
  uint8_t cp = (id >> 16) & 0xff;

  Trace(1, "Mf %d, Type %d, Cap %d", mf, tp, cp);

  unsigned sz = hal_SpiFlashGetSize();
  unsigned bs = hal_SpiFlashGetBlockSize();
  unsigned ss = hal_SpiFlashGetSectorSize();
  Trace(1, "Size %dk, Block %dk, Sector %dk", sz / 1024, bs / 1024, ss / 1024);

  flashblock = ss;

  hal_SpiFlashSetCheckIrq(true);

  void* ptr;

//Backup
  ptr = hal_SpiFlashMapUncacheAddress(0);
  SaveBlock(ptr,64*1024,"/t/bootloader.img");

  ptr = hal_SpiFlashMapUncacheAddress(0x100000);
  SaveBlock(ptr,0x140000,"/t/platform.img");

//No point really! this is us!
  ptr = hal_SpiFlashMapUncacheAddress(0x240000);
  SaveBlock(ptr,1*1024*1024,"/t/app.img");

  ptr = hal_SpiFlashMapUncacheAddress(0x390000); //
  SaveBlock(ptr,408*1024,"/t/spiffs.img");

  ptr = hal_SpiFlashMapUncacheAddress(0x3FA000); //
  SaveBlock(ptr,8*1024,"/t/calib1.img");
  ptr = hal_SpiFlashMapUncacheAddress(0x3FC000); //
  SaveBlock(ptr,8*1024,"/t/calib2.img");

  ptr = hal_SpiFlashMapUncacheAddress(0x3FE000);
  SaveBlock(ptr,8*1024,"/t/factorycfg.img");

/*      ptr = hal_SpiFlashMapUncacheAddress(0x3FE000);
      DumpBlock(ptr, 8*1024);*/

//  WriteFlash("/t/fs.img", 0x390000, 408 * 1024); // Filesystem
//  WriteFlash("/t/app.img",  0x240000, 1*1024*1024 ); //App is the flashed app
//  WriteFlash("/t/cal1.img", 0x3FA000, 8*1024 );
//  WriteFlash("/t/cal2.img", 0x3FC000, 8*1024 );
// OS_Sleep(1000); PM_Reboot();

  while (1) {
    Trace(1, "Dumped to SD");
    OS_Sleep(30000);
  }
}

void MainTask(void* pData) {
  API_Event_t* event = NULL;

  secondTaskHandle = OS_CreateTask(SecondTask, NULL, NULL, SECOND_TASK_STACK_SIZE, SECOND_TASK_PRIORITY, 0, 0, SECOND_TASK_NAME);

  while (1) {
    if (OS_WaitEvent(mainTaskHandle, (void**)&event, OS_TIME_OUT_WAIT_FOREVER)) {
      EventDispatch(event);
      OS_Free(event->pParam1);
      OS_Free(event->pParam2);
      OS_Free(event);
    }
  }
}

void first_Main(void) {
  mainTaskHandle = OS_CreateTask(MainTask, NULL, NULL, MAIN_TASK_STACK_SIZE, MAIN_TASK_PRIORITY, 0, 0, MAIN_TASK_NAME);
  OS_SetUserMainHandle(&mainTaskHandle);
}