#include <cstdint>
#include <initializer_list>

#include "driver/flash_loader_defs.hpp"

#define NRF_FLASH_LOADER_DEBUG 0

#ifdef __cplusplus
  #define   __I     volatile             /*!< Defines 'read only' permissions */
#else
  #define   __I     volatile const       /*!< Defines 'read only' permissions */
#endif
#define     __O     volatile             /*!< Defines 'write only' permissions */
#define     __IO    volatile             /*!< Defines 'read / write' permissions */

/* following defines should be used for structure members */
#define     __IM     volatile const      /*! Defines 'read only' structure member permissions */
#define     __OM     volatile            /*! Defines 'write only' structure member permissions */
#define     __IOM    volatile            /*! Defines 'read / write' structure member permissions */

/* Bit 0 : Disable the protection mechanism for NVM regions while in debug interface mode. This register will only disable the protection mechanism if the device is in debug interface mode. */
#define BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Pos (0UL) /*!< Position of DISABLEINDEBUG field. */
#define BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Msk (0x1UL << BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Pos) /*!< Bit mask of DISABLEINDEBUG field. */
#define BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Enabled (0UL) /*!< Enable in debug */
#define BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Disabled (1UL) /*!< Disable in debug */

/* Peripheral: NVMC */
/* Description: Non Volatile Memory Controller */

/* Register: NVMC_READY */
/* Description: Ready flag */

/* Bit 0 : NVMC is ready or busy */
#define NVMC_READY_READY_Pos (0UL) /*!< Position of READY field. */
#define NVMC_READY_READY_Msk (0x1UL << NVMC_READY_READY_Pos) /*!< Bit mask of READY field. */
#define NVMC_READY_READY_Busy (0UL) /*!< NVMC is busy (on-going write or erase operation) */
#define NVMC_READY_READY_Ready (1UL) /*!< NVMC is ready */

/* Register: NVMC_CONFIG */
/* Description: Configuration register */

/* Bits 1..0 : Program memory access mode. It is strongly recommended to only activate erase and write modes when they are actively used. Enabling write or erase will invalidate the cache and keep it invalidated. */
#define NVMC_CONFIG_WEN_Pos (0UL) /*!< Position of WEN field. */
#define NVMC_CONFIG_WEN_Msk (0x3UL << NVMC_CONFIG_WEN_Pos) /*!< Bit mask of WEN field. */
#define NVMC_CONFIG_WEN_Ren (0UL) /*!< Read only access */
#define NVMC_CONFIG_WEN_Wen (1UL) /*!< Write Enabled */
#define NVMC_CONFIG_WEN_Een (2UL) /*!< Erase enabled */

/* Register: NVMC_ERASEPAGE */
/* Description: Register for erasing a page in Code area */

/* Bits 31..0 : Register for starting erase of a page in Code area */
#define NVMC_ERASEPAGE_ERASEPAGE_Pos (0UL) /*!< Position of ERASEPAGE field. */
#define NVMC_ERASEPAGE_ERASEPAGE_Msk (0xFFFFFFFFUL << NVMC_ERASEPAGE_ERASEPAGE_Pos) /*!< Bit mask of ERASEPAGE field. */

/* Register: NVMC_ERASEPCR1 */
/* Description: Deprecated register -  Register for erasing a page in Code area. Equivalent to ERASEPAGE. */

/* Bits 31..0 : Register for erasing a page in Code area. Equivalent to ERASEPAGE. */
#define NVMC_ERASEPCR1_ERASEPCR1_Pos (0UL) /*!< Position of ERASEPCR1 field. */
#define NVMC_ERASEPCR1_ERASEPCR1_Msk (0xFFFFFFFFUL << NVMC_ERASEPCR1_ERASEPCR1_Pos) /*!< Bit mask of ERASEPCR1 field. */

/* Register: NVMC_ERASEALL */
/* Description: Register for erasing all non-volatile user memory */

/* Bit 0 : Erase all non-volatile memory including UICR registers. Note that code erase has to be enabled by CONFIG.EEN before the UICR can be erased. */
#define NVMC_ERASEALL_ERASEALL_Pos (0UL) /*!< Position of ERASEALL field. */
#define NVMC_ERASEALL_ERASEALL_Msk (0x1UL << NVMC_ERASEALL_ERASEALL_Pos) /*!< Bit mask of ERASEALL field. */
#define NVMC_ERASEALL_ERASEALL_NoOperation (0UL) /*!< No operation */
#define NVMC_ERASEALL_ERASEALL_Erase (1UL) /*!< Start chip erase */

/* Register: NVMC_ERASEPCR0 */
/* Description: Deprecated register -  Register for erasing a page in Code area. Equivalent to ERASEPAGE. */

/* Bits 31..0 : Register for starting erase of a page in Code area. Equivalent to ERASEPAGE. */
#define NVMC_ERASEPCR0_ERASEPCR0_Pos (0UL) /*!< Position of ERASEPCR0 field. */
#define NVMC_ERASEPCR0_ERASEPCR0_Msk (0xFFFFFFFFUL << NVMC_ERASEPCR0_ERASEPCR0_Pos) /*!< Bit mask of ERASEPCR0 field. */

/* Register: NVMC_ERASEUICR */
/* Description: Register for erasing User Information Configuration Registers */

/* Bit 0 : Register starting erase of all User Information Configuration Registers. Note that code erase has to be enabled by CONFIG.EEN before the UICR can be erased. */
#define NVMC_ERASEUICR_ERASEUICR_Pos (0UL) /*!< Position of ERASEUICR field. */
#define NVMC_ERASEUICR_ERASEUICR_Msk (0x1UL << NVMC_ERASEUICR_ERASEUICR_Pos) /*!< Bit mask of ERASEUICR field. */
#define NVMC_ERASEUICR_ERASEUICR_NoOperation (0UL) /*!< No operation */
#define NVMC_ERASEUICR_ERASEUICR_Erase (1UL) /*!< Start erase of UICR */

/* Register: NVMC_ICACHECNF */
/* Description: I-Code cache configuration register. */

/* Bit 8 : Cache profiling enable */
#define NVMC_ICACHECNF_CACHEPROFEN_Pos (8UL) /*!< Position of CACHEPROFEN field. */
#define NVMC_ICACHECNF_CACHEPROFEN_Msk (0x1UL << NVMC_ICACHECNF_CACHEPROFEN_Pos) /*!< Bit mask of CACHEPROFEN field. */
#define NVMC_ICACHECNF_CACHEPROFEN_Disabled (0UL) /*!< Disable cache profiling */
#define NVMC_ICACHECNF_CACHEPROFEN_Enabled (1UL) /*!< Enable cache profiling */

/* Bit 0 : Cache enable */
#define NVMC_ICACHECNF_CACHEEN_Pos (0UL) /*!< Position of CACHEEN field. */
#define NVMC_ICACHECNF_CACHEEN_Msk (0x1UL << NVMC_ICACHECNF_CACHEEN_Pos) /*!< Bit mask of CACHEEN field. */
#define NVMC_ICACHECNF_CACHEEN_Disabled (0UL) /*!< Disable cache. Invalidates all cache entries. */
#define NVMC_ICACHECNF_CACHEEN_Enabled (1UL) /*!< Enable cache */

/* Register: NVMC_IHIT */
/* Description: I-Code cache hit counter. */

/* Bits 31..0 : Number of cache hits */
#define NVMC_IHIT_HITS_Pos (0UL) /*!< Position of HITS field. */
#define NVMC_IHIT_HITS_Msk (0xFFFFFFFFUL << NVMC_IHIT_HITS_Pos) /*!< Bit mask of HITS field. */

/* Register: NVMC_IMISS */
/* Description: I-Code cache miss counter. */

/* Bits 31..0 : Number of cache misses */
#define NVMC_IMISS_MISSES_Pos (0UL) /*!< Position of MISSES field. */
#define NVMC_IMISS_MISSES_Msk (0xFFFFFFFFUL << NVMC_IMISS_MISSES_Pos) /*!< Bit mask of MISSES field. */

__attribute__((always_inline)) static inline void __ISB(void)
{
  __asm volatile ("isb 0xF":::"memory");
}


/**
  \brief   Data Synchronization Barrier
  \details Acts as a special kind of Data Memory Barrier.
           It completes when all explicit memory accesses before this instruction complete.
 */
__attribute__((always_inline)) static inline void __DSB(void)
{
  __asm volatile ("dsb 0xF":::"memory");
}

/**
  * @brief Block Protect (BPROT)
  */

typedef struct {                                /*!< (@ 0x40000000) BPROT Structure                                            */
  __IM  uint32_t  RESERVED[384];
  __IOM uint32_t  CONFIG0;                      /*!< (@ 0x00000600) Block protect configuration register 0                     */
  __IOM uint32_t  CONFIG1;                      /*!< (@ 0x00000604) Block protect configuration register 1                     */
  __IOM uint32_t  DISABLEINDEBUG;               /*!< (@ 0x00000608) Disable protection mechanism in debug interface
                                                                    mode                                                       */
  __IOM uint32_t  UNUSED0;                      /*!< (@ 0x0000060C) Unspecified                                                */
  __IOM uint32_t  CONFIG2;                      /*!< (@ 0x00000610) Block protect configuration register 2                     */
  __IOM uint32_t  CONFIG3;                      /*!< (@ 0x00000614) Block protect configuration register 3                     */
} NRF_BPROT_Type;                               /*!< Size = 1560 (0x618)                                                       */

/**
  * @brief Non Volatile Memory Controller (NVMC)
  */

typedef struct {                                /*!< (@ 0x4001E000) NVMC Structure                                             */
  __IM  uint32_t  RESERVED[256];
  __IM  uint32_t  READY;                        /*!< (@ 0x00000400) Ready flag                                                 */
  __IM  uint32_t  RESERVED1[64];
  __IOM uint32_t  CONFIG;                       /*!< (@ 0x00000504) Configuration register                                     */

  union {
    __IOM uint32_t ERASEPAGE;                   /*!< (@ 0x00000508) Register for erasing a page in Code area                   */
    __IOM uint32_t ERASEPCR1;                   /*!< (@ 0x00000508) Deprecated register - Register for erasing a
                                                                    page in Code area. Equivalent to ERASEPAGE.                */
  };
  __IOM uint32_t  ERASEALL;                     /*!< (@ 0x0000050C) Register for erasing all non-volatile user memory          */
  __IOM uint32_t  ERASEPCR0;                    /*!< (@ 0x00000510) Deprecated register - Register for erasing a
                                                                    page in Code area. Equivalent to ERASEPAGE.                */
  __IOM uint32_t  ERASEUICR;                    /*!< (@ 0x00000514) Register for erasing User Information Configuration
                                                                    Registers                                                  */
  __IM  uint32_t  RESERVED2[10];
  __IOM uint32_t  ICACHECNF;                    /*!< (@ 0x00000540) I-Code cache configuration register.                       */
  __IM  uint32_t  RESERVED3;
  __IOM uint32_t  IHIT;                         /*!< (@ 0x00000548) I-Code cache hit counter.                                  */
  __IOM uint32_t  IMISS;                        /*!< (@ 0x0000054C) I-Code cache miss counter.                                 */
} NRF_NVMC_Type;                                /*!< Size = 1360 (0x550)                                                       */

typedef struct {                                /*!< (@ 0x50000000) P0 Structure                                               */
  __IM  uint32_t  RESERVED[321];
  __IOM uint32_t  OUT;                          /*!< (@ 0x00000504) Write GPIO port                                            */
  __IOM uint32_t  OUTSET;                       /*!< (@ 0x00000508) Set individual bits in GPIO port                           */
  __IOM uint32_t  OUTCLR;                       /*!< (@ 0x0000050C) Clear individual bits in GPIO port                         */
  __IM  uint32_t  IN;                           /*!< (@ 0x00000510) Read GPIO port                                             */
  __IOM uint32_t  DIR;                          /*!< (@ 0x00000514) Direction of GPIO pins                                     */
  __IOM uint32_t  DIRSET;                       /*!< (@ 0x00000518) DIR set register                                           */
  __IOM uint32_t  DIRCLR;                       /*!< (@ 0x0000051C) DIR clear register                                         */
  __IOM uint32_t  LATCH;                        /*!< (@ 0x00000520) Latch register indicating what GPIO pins that
                                                                    have met the criteria set in the PIN_CNF[n].SENSE
                                                                    registers                                                  */
  __IOM uint32_t  DETECTMODE;                   /*!< (@ 0x00000524) Select between default DETECT signal behaviour
                                                                    and LDETECT mode                                           */
  __IM  uint32_t  RESERVED1[118];
  __IOM uint32_t  PIN_CNF[32];                  /*!< (@ 0x00000700) Description collection[0]: Configuration of GPIO
                                                                    pins                                                       */
} NRF_GPIO_Type;                                /*!< Size = 1920 (0x780)                                                       */

#define NRF_NVMC_BASE               0x4001E000UL
#define NRF_BPROT_BASE              0x40000000UL
#define NRF_P0_BASE                 0x50000000UL

#define NRF_NVMC                    ((NRF_NVMC_Type*)          NRF_NVMC_BASE)
#define NRF_BPROT                   ((NRF_BPROT_Type*)         NRF_BPROT_BASE)
#define NRF_P0                      ((NRF_GPIO_Type*)          NRF_P0_BASE)
#define NRF_GPIO                    ((NRF_GPIO_Type*)          NRF_P0_BASE)

/* Register: GPIO_PIN_CNF */
/* Description: Description collection[0]:  Configuration of GPIO pins */

/* Bits 17..16 : Pin sensing mechanism */
#define GPIO_PIN_CNF_SENSE_Pos (16UL) /*!< Position of SENSE field. */
#define GPIO_PIN_CNF_SENSE_Msk (0x3UL << GPIO_PIN_CNF_SENSE_Pos) /*!< Bit mask of SENSE field. */
#define GPIO_PIN_CNF_SENSE_Disabled (0UL) /*!< Disabled */
#define GPIO_PIN_CNF_SENSE_High (2UL) /*!< Sense for high level */
#define GPIO_PIN_CNF_SENSE_Low (3UL) /*!< Sense for low level */

/* Bits 10..8 : Drive configuration */
#define GPIO_PIN_CNF_DRIVE_Pos (8UL) /*!< Position of DRIVE field. */
#define GPIO_PIN_CNF_DRIVE_Msk (0x7UL << GPIO_PIN_CNF_DRIVE_Pos) /*!< Bit mask of DRIVE field. */
#define GPIO_PIN_CNF_DRIVE_S0S1 (0UL) /*!< Standard '0', standard '1' */
#define GPIO_PIN_CNF_DRIVE_H0S1 (1UL) /*!< High drive '0', standard '1' */
#define GPIO_PIN_CNF_DRIVE_S0H1 (2UL) /*!< Standard '0', high drive '1' */
#define GPIO_PIN_CNF_DRIVE_H0H1 (3UL) /*!< High drive '0', high 'drive '1'' */
#define GPIO_PIN_CNF_DRIVE_D0S1 (4UL) /*!< Disconnect '0' standard '1' (normally used for wired-or connections) */
#define GPIO_PIN_CNF_DRIVE_D0H1 (5UL) /*!< Disconnect '0', high drive '1' (normally used for wired-or connections) */
#define GPIO_PIN_CNF_DRIVE_S0D1 (6UL) /*!< Standard '0'. disconnect '1' (normally used for wired-and connections) */
#define GPIO_PIN_CNF_DRIVE_H0D1 (7UL) /*!< High drive '0', disconnect '1' (normally used for wired-and connections) */

/* Bits 3..2 : Pull configuration */
#define GPIO_PIN_CNF_PULL_Pos (2UL) /*!< Position of PULL field. */
#define GPIO_PIN_CNF_PULL_Msk (0x3UL << GPIO_PIN_CNF_PULL_Pos) /*!< Bit mask of PULL field. */
#define GPIO_PIN_CNF_PULL_Disabled (0UL) /*!< No pull */
#define GPIO_PIN_CNF_PULL_Pulldown (1UL) /*!< Pull down on pin */
#define GPIO_PIN_CNF_PULL_Pullup (3UL) /*!< Pull up on pin */

/* Bit 1 : Connect or disconnect input buffer */
#define GPIO_PIN_CNF_INPUT_Pos (1UL) /*!< Position of INPUT field. */
#define GPIO_PIN_CNF_INPUT_Msk (0x1UL << GPIO_PIN_CNF_INPUT_Pos) /*!< Bit mask of INPUT field. */
#define GPIO_PIN_CNF_INPUT_Connect (0UL) /*!< Connect input buffer */
#define GPIO_PIN_CNF_INPUT_Disconnect (1UL) /*!< Disconnect input buffer */

/* Bit 0 : Pin direction. Same physical register as DIR register */
#define GPIO_PIN_CNF_DIR_Pos (0UL) /*!< Position of DIR field. */
#define GPIO_PIN_CNF_DIR_Msk (0x1UL << GPIO_PIN_CNF_DIR_Pos) /*!< Bit mask of DIR field. */
#define GPIO_PIN_CNF_DIR_Input (0UL) /*!< Configure pin as an input pin */
#define GPIO_PIN_CNF_DIR_Output (1UL) /*!< Configure pin as an output pin */

#if NRF_FLASH_LOADER_DEBUG
  static constexpr std::uint32_t debug_pin0 = 19;
  static constexpr std::uint32_t debug_pin1 = 20;
  static constexpr std::uint32_t debug_pin2 = 22;
  static constexpr std::uint32_t debug_pin3 = 23;
  static constexpr std::uint32_t all_debug_pin_mask =
      ( 1 << debug_pin0 ) |
      ( 1 << debug_pin1 ) |
      ( 1 << debug_pin2 ) |
      ( 1 << debug_pin3 );
#endif

static constexpr std::uint32_t    flash_driver_result_ok = 0;
static constexpr std::uint32_t    flash_driver_result_error = 1;

/**
 * @brief writes a page of flash memory
 *
 * @param word_count size of flash memory / 4
 * @param target_address start address of flash memory
 * @param content_address start address, containing the content to be flashed
 */
extern "C" std::uint32_t flash_loader(
            std::uint32_t function,
            std::uint32_t word_count,
            std::uint32_t target_address,
            std::uint32_t content_address )
{
    bool result = true;

#   if NRF_FLASH_LOADER_DEBUG
      for ( auto pin: { debug_pin0, debug_pin1, debug_pin2, debug_pin3 } )
          NRF_GPIO->PIN_CNF[ pin ] =
              ( GPIO_PIN_CNF_DRIVE_S0H1 << GPIO_PIN_CNF_DRIVE_Pos ) |
              ( GPIO_PIN_CNF_DIR_Output << GPIO_PIN_CNF_DIR_Pos );

      NRF_GPIO->OUTSET = 1 << debug_pin0;
#   endif

    // disable cache
    NRF_NVMC->ICACHECNF = 0;
    // Disable Blockprotection
    NRF_BPROT->DISABLEINDEBUG = BPROT_DISABLEINDEBUG_DISABLEINDEBUG_Msk;

    if ( static_cast< driver::flash_loader_function >( function ) == driver::flash_loader_function::erase_all )
    {
#       if NRF_FLASH_LOADER_DEBUG
            NRF_GPIO->OUTSET = 1 << debug_pin1;
#       endif

        NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Een;
        __ISB();
        __DSB();

        NRF_NVMC->ERASEALL = NVMC_ERASEALL_ERASEALL_Erase;
        while (NRF_NVMC->READY == NVMC_READY_READY_Busy)
            ;

#       if NRF_FLASH_LOADER_DEBUG
            NRF_GPIO->OUTCLR = 1 << debug_pin1;
            NRF_GPIO->OUTCLR = 1 << debug_pin0;
#       endif

        return flash_driver_result_ok;
    }

    if ( static_cast< driver::flash_loader_function >( function ) == driver::flash_loader_function::flash_and_erase_block )
    {
#       if NRF_FLASH_LOADER_DEBUG
            NRF_GPIO->OUTSET = 1 << debug_pin2;
#       endif

        NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Een;
        __ISB();
        __DSB();

        NRF_NVMC->ERASEPAGE = target_address;
        while (NRF_NVMC->READY == NVMC_READY_READY_Busy)
            ;

#       if NRF_FLASH_LOADER_DEBUG
            NRF_GPIO->OUTCLR = 1 << debug_pin2;
#       endif
    }

    NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen;
    __ISB();
    __DSB();

    for ( ; word_count && result; --word_count )
    {
        *reinterpret_cast< std::uint32_t* >( target_address ) =
            *reinterpret_cast< const std::uint32_t* >( content_address );

        while (NRF_NVMC->READY == NVMC_READY_READY_Busy)
            ;

        result = *reinterpret_cast< std::uint32_t* >( target_address ) ==
            *reinterpret_cast< const std::uint32_t* >( content_address );

        target_address += 4;
        content_address += 4;
    }

    NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren;
    __ISB();
    __DSB();

#   if NRF_FLASH_LOADER_DEBUG
        NRF_GPIO->OUTCLR = 1 << debug_pin0;
#   endif

    return result ? flash_driver_result_ok : flash_driver_result_error;
}