#include "device.h"
  #include "regs/regs.h"
  extern "C" {
  #include <assert.h>
  }
  #include <ion.h>
  #include "led.h"
  #include "display.h"
  #include "keyboard.h"
  #include "battery.h"
  #include "sd_card.h"
  #include "backlight.h"
  #include "console.h"
  #include "swd.h"
  #include "usb.h"
  #include "bench/bench.h"
  #include "base64.h"
  
  #define USE_SD_CARD 0
  
  // Public Ion methods
  
  /* TODO: The delay methods 'msleep' and 'usleep' are currently dependent on the
   * optimizations chosen by the compiler. To prevent that and to gain in
   * precision, we could use the controller cycle counter (Systick). */
  
  void Ion::msleep(long ms) {
    for (volatile long i=0; i<8852*ms; i++) {
        __asm volatile("nop");
    }
  }
  void Ion::usleep(long us) {
    for (volatile long i=0; i<9*us; i++) {
      __asm volatile("nop");
    }
  }
  
  uint32_t Ion::crc32(const uint32_t * data, size_t length) {
    bool initialCRCEngineState = RCC.AHB1ENR()->getCRCEN();
    RCC.AHB1ENR()->setCRCEN(true);
    CRC.CR()->setRESET(true);
  
    const uint32_t * end = data + length;
    while (data < end) {
      CRC.DR()->set(*data++);
    }
  
    uint32_t result = CRC.DR()->get();
    RCC.AHB1ENR()->setCRCEN(initialCRCEngineState);
    return result;
  }
  
  uint32_t Ion::random() {
    bool initialRNGEngineState = RCC.AHB2ENR()->getRNGEN();
    RCC.AHB2ENR()->setRNGEN(true);
  
    RNG.CR()->setRNGEN(true);
  
    while (RNG.SR()->getDRDY() == 0) {
    }
    uint32_t result = RNG.DR()->get();
  
    RNG.CR()->setRNGEN(false);
    RCC.AHB2ENR()->setRNGEN(initialRNGEngineState);
  
    return result;
  }
  
  void Ion::Device::copySerialNumber(char * buffer) {
    const unsigned char * rawUniqueID = (const unsigned char *)0x1FFF7A10;
    Base64::encode(rawUniqueID, 12, buffer);
    buffer[SerialNumberLength] = 0;
  }
  
  const char * Ion::serialNumber() {
    static char serialNumber[Device::SerialNumberLength + 1] = {0};
    if (serialNumber[0] == 0) {
      Device::copySerialNumber(serialNumber);
    }
    return serialNumber;
  }
  
  const char * Ion::fccId() {
    return "2ALWP-N0100";
  }
  
  // Private Ion::Device methods
  
  namespace Ion {
  namespace Device {
  
  void initFPU() {
    // http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0553a/BABDBFBJ.html
    CM4.CPACR()->setAccess(10, CM4::CPACR::Access::Full);
    CM4.CPACR()->setAccess(11, CM4::CPACR::Access::Full);
    // FIXME: The pipeline should be flushed at this point
  }
  
  void coreReset() {
    // Perform a full core reset
    CM4.AIRCR()->requestReset();
  }
  
  void jumpReset() {
    uint32_t * stackPointerAddress = reinterpret_cast<uint32_t *>(0x08000000);
    uint32_t * resetHandlerAddress = reinterpret_cast<uint32_t *>(0x08000004);
  
    /* Jump to the reset service routine after having reset the stack pointer.
     * Both addresses are fetched from the base of the Flash memory, just like a
     * real reset would. These operations should be made at once, otherwise the C
     * compiler might emit some instructions that modify the stack inbetween. */
  
    asm volatile (
        "msr MSP, %[stackPointer] ; bx %[resetHandler]"
        : :
        [stackPointer] "r" (*stackPointerAddress),
        [resetHandler] "r" (*resetHandlerAddress)
    );
  }
  
  void init() {
    initClocks();
  
    // Ensure right location of interrupt vectors
    // The bootloader leaves its own after flashing
    SYSCFG.MEMRMP()->setMEM_MODE(SYSCFG::MEMRMP::MemMode::MainFlashmemory);
    CM4.VTOR()->setVTOR((void*) 0);
  
    // Put all inputs as Analog Input, No pull-up nor pull-down
    // Except for the SWD port (PB3, PA13, PA14)
    GPIOA.MODER()->set(0xEBFFFFFF);
    GPIOA.PUPDR()->set(0x24000000);
    GPIOB.MODER()->set(0xFFFFFFBF);
    GPIOB.PUPDR()->set(0x00000000);
    for (int g=2; g<5; g++) {
      GPIO(g).MODER()->set(0xFFFFFFFF); // All to "Analog"
      GPIO(g).PUPDR()->set(0x00000000); // All to "None"
    }
  
  #if EPSILON_DEVICE_BENCH
    bool consolePeerConnectedOnBoot = Ion::Console::Device::peerConnected();
  #endif
  
    initPeripherals();
  
  #if EPSILON_DEVICE_BENCH
    if (consolePeerConnectedOnBoot) {
      Ion::Device::Bench::run();
    }
  #endif
  }
  
  void shutdown() {
    shutdownPeripherals();
    shutdownClocks();
  }
  
  void initPeripherals() {
    Display::Device::init();
    Backlight::Device::init();
    Keyboard::Device::init();
    LED::Device::init();
    Battery::Device::init();
    USB::Device::init();
  #if USE_SD_CARD
    SDCard::Device::init();
  #endif
    Console::Device::init();
    SWD::Device::init();
  }
  
  void shutdownPeripherals(bool keepLEDAwake) {
    SWD::Device::shutdown();
    Console::Device::shutdown();
  #if USE_SD_CARD
    SDCard::Device::shutdown();
  #endif
    USB::Device::shutdown();
    Battery::Device::shutdown();
    if (!keepLEDAwake) {
      LED::Device::shutdown();
    }
    Keyboard::Device::shutdown();
    Backlight::Device::shutdown();
    Display::Device::shutdown();
  }
  
  void initClocks() {
    /* System clock
     * Configure the CPU at 96 MHz, APB2 and USB at 48 MHz. */
  
    /* After reset the Flash runs as fast as the CPU. When we clock the CPU faster
     * the flash memory cannot follow and therefore flash memory accesses need to
     * wait a little bit.
     * The spec tells us that at 2.8V and over 90MHz the flash expects 3 WS. */
    FLASH.ACR()->setLATENCY(3);
  
    /* Enable prefetching flash instructions */
    /* Fetching instructions increases slightly the power consumption but the
     * increase is negligible compared to the screen consumption. */
    FLASH.ACR()->setPRFTEN(true);
  
    /* Set flash instruction and data cache */
    FLASH.ACR()->setDCEN(true);
    FLASH.ACR()->setICEN(true);
  
    /* After reset, the device is using the high-speed internal oscillator (HSI)
     * as a clock source, which runs at a fixed 16 MHz frequency. The HSI is not
     * accurate enough for reliable USB operation, so we need to use the external
     * high-speed oscillator (HSE). */
  
    // Enable the HSE and wait for it to be ready
    RCC.CR()->setHSEON(true);
    while(!RCC.CR()->getHSERDY()) {
    }
  
    /* Given the crystal used on our device, the HSE will oscillate at 25 MHz. By
     * piping it through a phase-locked loop (PLL) we can derive other frequencies
     * for use in different parts of the system. Combining the default PLL values
     * with a PLLM of 25 and a PLLQ of 4 yields both a 96 MHz frequency for SYSCLK
     * and the required 48 MHz USB clock. */
  
    // Configure the PLL ratios and use HSE as a PLL input
    RCC.PLLCFGR()->setPLLM(25);
    RCC.PLLCFGR()->setPLLQ(4);
    RCC.PLLCFGR()->setPLLSRC(RCC::PLLCFGR::PLLSRC::HSE);
    // 96 MHz is too fast for APB1. Divide it by two to reach 48 MHz
    RCC.CFGR()->setPPRE1(RCC::CFGR::APBPrescaler::AHBDividedBy2);
  
    /* If you want to considerably slow down the whole machine uniformely, which
     * can be very useful to diagnose performance issues, just uncomment the line
     * below. Note that even booting takes a few seconds, so don't be surprised
     * if the screen is black for a short while upon booting. */
    // RCC.CFGR()->setHPRE(RCC::CFGR::AHBPrescaler::SysClkDividedBy128);
  
    // Enable the PLL and wait for it to be ready
    RCC.CR()->setPLLON(true);
    while(!RCC.CR()->getPLLRDY()) {
    }
  
    // Use the PLL output as a SYSCLK source
    RCC.CFGR()->setSW(RCC::CFGR::SW::PLL);
    while (RCC.CFGR()->getSWS() != RCC::CFGR::SW::PLL) {
    }
  
    // Now that we don't need use it anymore, turn the HSI off
    RCC.CR()->setHSION(false);
  
    // Peripheral clocks
  
    // AHB1 bus
    // Our peripherals are using GPIO A, B, C, D and E.
    // We're not using the CRC nor DMA engines.
    class RCC::AHB1ENR ahb1enr(0); // Reset value
    ahb1enr.setGPIOAEN(true);
    ahb1enr.setGPIOBEN(true);
    ahb1enr.setGPIOCEN(true);
    ahb1enr.setGPIODEN(true);
    ahb1enr.setGPIOEEN(true);
    ahb1enr.setDMA2EN(true);
    RCC.AHB1ENR()->set(ahb1enr);
  
    // AHB2 bus
    RCC.AHB2ENR()->setOTGFSEN(true);
  
    // AHB3 bus
    RCC.AHB3ENR()->setFSMCEN(true);
  
    // APB1 bus
    // We're using TIM3 for the LEDs
    RCC.APB1ENR()->setTIM3EN(true);
    RCC.APB1ENR()->setPWREN(true);
  
    // APB2 bus
    class RCC::APB2ENR apb2enr(0x00008000); // Reset value
    apb2enr.setADC1EN(true);
    apb2enr.setSYSCFGEN(true);
  #if USE_SD_CARD
    apb2enr.setSDIOEN(true);
  #endif
    RCC.APB2ENR()->set(apb2enr);
  }
  
  void shutdownClocks(bool keepLEDAwake) {
    // APB2 bus
    RCC.APB2ENR()->set(0x00008000); // Reset value
  
    // APB1
    class RCC::APB1ENR apb1enr(0x00000400); // Reset value
    // AHB1 bus
    class RCC::AHB1ENR ahb1enr(0); // Reset value
    if (keepLEDAwake) {
      apb1enr.setTIM3EN(true);
      ahb1enr.setGPIOBEN(true);
      ahb1enr.setGPIOCEN(true);
    }
    RCC.APB1ENR()->set(apb1enr);
    RCC.AHB1ENR()->set(ahb1enr);
  
    RCC.AHB3ENR()->setFSMCEN(false);
  }
  
  }
  }