/**
 * Fills eeprom area with pattern, than read it back and compare
 */
static void __block_api_write(uint8_t pattern, uint32_t len, uint8_t misaligment){
  uint32_t i = 0;
  uint32_t status = 0;
  uint32_t pos = 0;

  /* fill buffer with pattern */
  for (i = 0; i < len; i++)
    referencebuf[i] = pattern;

  /* move to begin of test area */
  chFileStreamSeek(&EfsTest, TestAreaStart + misaligment);
  pos = chFileStreamGetPosition(&EfsTest);
  if (pos != TestAreaStart + misaligment)
    chDbgPanic("file seek error");

  /* write */
  status = chFileStreamWrite(&EfsTest, referencebuf, len);
  if (status < len)
    chDbgPanic("write failed");

  /* check */
  pos = chFileStreamGetPosition(&EfsTest);
  if (pos != len + TestAreaStart + misaligment)
    chDbgPanic("file seek error");
  chFileStreamSeek(&EfsTest, pos - len);
  status = chFileStreamRead(&EfsTest, checkbuf, len);
  if (status < len)
    chDbgPanic("veryfication failed");
  if (memcmp(referencebuf, checkbuf, len) != 0)
    chDbgPanic("veryfication failed");
}

/**
 * Fills eeprom area with pattern, than read it back and compare
 */
static void pattern_fill(EepromFileStream *EfsTest, uint8_t pattern){
  uint32_t i = 0;
  uint32_t status = 0;
  uint32_t pos = 0;
  uint32_t len = chFileStreamGetSize(EfsTest);

  /* fill buffer with pattern */
  for (i = 0; i < len; i++)
    referencebuf[i] = pattern;

  /* move to begin of test area */
  pos = 0;
  chFileStreamSeek(EfsTest, pos);
  if (pos != chFileStreamGetPosition(EfsTest))
    chDbgPanic("file seek error");

  /* write */
  status = chFileStreamWrite(EfsTest, referencebuf, len);
  if (status != len)
    chDbgPanic("write failed");

  /* check */
  pos = chFileStreamGetPosition(EfsTest);
  if (pos != len)
    chDbgPanic("writing error");

  pos = 0;
  chFileStreamSeek(EfsTest, pos);
  status = chFileStreamRead(EfsTest, checkbuf, len);
  if (status != len)
    chDbgPanic("reading back failed");
  if (memcmp(referencebuf, checkbuf, len) != 0)
    chDbgPanic("veryfication failed");
}

void MicrosdInit(void){

  /*
   * Initializes the MMC driver to work with SPI.
   */
  palSetPadMode(IOPORT1, PIOA_SPI0_NSS, PAL_MODE_OUTPUT_PUSHPULL);
  palSetPad(IOPORT1, PIOA_SPI0_NSS);

  microsd_writer_tp = chThdCreateStatic(
      MmcWriterThreadWA,
      sizeof(MmcWriterThreadWA),
      MMC_THREAD_PRIO,
      MmcWriterThread,
      NULL);
  if (microsd_writer_tp == NULL)
    chDbgPanic("Can not allocate memory");

  microsd_reader_cc_tp = chThdCreateStatic(
      MmcReaderCcThreadWA,
      sizeof(MmcReaderCcThreadWA),
      MMC_THREAD_PRIO,
      MmcReaderCcThread,
      &SDGSM);
  if (microsd_reader_cc_tp == NULL)
      chDbgPanic("Can not allocate memory");

  microsd_reader_dm_tp = chThdCreateStatic(
      MmcReaderDmThreadWA,
      sizeof(MmcReaderDmThreadWA),
      MMC_THREAD_PRIO,
      MmcReaderDmThread,
      &SDDM);
  if (microsd_reader_dm_tp == NULL)
      chDbgPanic("Can not allocate memory");
}

static void test_wtapper_write_byte(uint8_t pattern, uint32_t pos){
  uint8_t u8result;
  uint8_t u8 = pattern;

  chFileStreamSeek(&EfsTest, pos);
  if (EepromWriteByte(&EfsTest, u8) != sizeof(u8))
    chDbgPanic("write failed");
  chFileStreamSeek(&EfsTest, chFileStreamGetPosition(&EfsTest) - sizeof(u8));
  u8result = EepromReadByte(&EfsTest);
  if (u8 != u8result)
    chDbgPanic("veryfication failed");
}

static void test_wtapper_write_halfword(uint8_t pattern, uint32_t pos){
  uint16_t u16result;
  uint16_t u16 = 0;
  u16 |= pattern << 8 | pattern;

  chFileStreamSeek(&EfsTest, pos);
  if (EepromWriteHalfword(&EfsTest, u16) != sizeof(u16))
    chDbgPanic("write failed");
  chFileStreamSeek(&EfsTest, chFileStreamGetPosition(&EfsTest) - sizeof(u16));
  u16result = EepromReadHalfword(&EfsTest);
  if (u16 != u16result)
    chDbgPanic("veryfication failed");
}

static void test_wtapper_write_word(uint8_t pattern, uint32_t pos){
  uint32_t u32result;
  uint32_t u32 = 0;
  u32 |= pattern << 24 | pattern << 16 | pattern << 8 | pattern;

  chFileStreamSeek(&EfsTest, pos);
  if (EepromWriteWord(&EfsTest, u32) != sizeof(u32))
    chDbgPanic("write failed");
  chFileStreamSeek(&EfsTest, chFileStreamGetPosition(&EfsTest) - sizeof(u32));
  u32result = EepromReadWord(&EfsTest);
  if (u32 != u32result)
    chDbgPanic("veryfication failed");
}

void eeprom_testsuit(void){
  int8_t i = 0;
  int8_t j = 0;
  int32_t n = 0;

  /* backup data from test area */
  chFileStreamSeek(&EfsTest, TestAreaStart);
  if (chFileStreamRead(&EfsTest, backupbuf, TEST_AREA_SIZE) < TEST_AREA_SIZE)
    chDbgPanic("backuping failed");

  /* first check the whole test area */
  test_api_all(TEST_AREA_SIZE, 0);

  for (i = -2; i < 3; i++){
    for (j = 0; j < 3; j++){
      /* large block tests */
      n = EEPROM_PAGE_SIZE;
      while (n < (TEST_AREA_SIZE - 3 * EEPROM_PAGE_SIZE)){
        test_api_all(n + i,  j);
        test_api_all(n + i,  EEPROM_PAGE_SIZE - 1 - j);
        n *= EEPROM_PAGE_SIZE;
      }
      /* small block tests */
      for (n = 2; n < 7; n++){
        test_api_all(n + i,     j);
        test_api_all(n + i,     EEPROM_PAGE_SIZE - 1 - j);
        test_api_all(n*10 + i,  j);
        test_api_all(n*10 + i,  EEPROM_PAGE_SIZE - 1 - j);
      }
    }
  }

  /* wrapper fucntions test */
  test_wrapper_write_all_patterns(TestAreaStart);
  test_wrapper_write_all_patterns(TestAreaStart + 1);
  for (i = -4; i < 5; i++)
    test_wrapper_write_all_patterns(TestAreaStart + EEPROM_PAGE_SIZE + i);
  test_wrapper_write_all_patterns(TestAreaFinish - 5);

  /* personally check end of test area */
  test_wtapper_write_byte    (0x55, TestAreaFinish - 1);
  test_wtapper_write_halfword(0xAA, TestAreaFinish - 2);
  test_wtapper_write_word    (0xA5, TestAreaFinish - 4);

  /* roll back data from backup */
  chFileStreamSeek(&EfsTest, TestAreaStart);
  if (chFileStreamWrite(&EfsTest, backupbuf, TEST_AREA_SIZE) < TEST_AREA_SIZE)
    chDbgPanic("rolling back failed");
}

/**
 * @brief   I2C shared ISR code.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 *
 * @notapi
 */
static void i2c_lld_serve_interrupt(I2CDriver *i2cp) {

  uint32_t sr;
  sr = AT91C_BASE_TWI->TWI_SR;
  /* this masking doing in official Atmel driver. Is it needed ??? */
  sr &= AT91C_BASE_TWI->TWI_IMR;

  if (sr & AT91C_TWI_NACK){
    i2cp->errors |= I2CD_ACK_FAILURE;
    wakeup_isr(i2cp, RDY_RESET);
    return;
  }
  if (sr & AT91C_TWI_RXRDY){
    _i2c_lld_serve_rx_interrupt(i2cp);
  }
  else if (sr & AT91C_TWI_TXRDY){
    _i2c_lld_serve_tx_interrupt(i2cp);
  }
  else if (sr & AT91C_TWI_TXCOMP){
    AT91C_BASE_TWI->TWI_IDR = AT91C_TWI_TXCOMP;
    wakeup_isr(i2cp, RDY_OK);
  }
  else
    chDbgPanic("Invalid value");
}

void ControllerInit(void){

  uint32_t *sysid;

  /* initial mavlink values */
  param_registry.valueSearch("SYS_ID",  &sysid);

  mavlink_system_struct.sysid  = *sysid;
  mavlink_system_struct.compid = MAV_COMP_ID_ALL;
  mavlink_system_struct.state  = MAV_STATE_BOOT;
  mavlink_system_struct.mode   = MAV_MODE_PREFLIGHT;
  mavlink_system_struct.type   = MAV_TYPE_FIXED_WING;

  /**/
  switch (mavlink_system_struct.type){
  case MAV_TYPE_GROUND_ROVER:
    mav_dispatcher.start(NORMALPRIO);
    cmd_executor.start(NORMALPRIO);
    break;

  case MAV_TYPE_FIXED_WING:
    mav_dispatcher.start(NORMALPRIO);
    cmd_executor.start(NORMALPRIO);
    break;

  default:
    chDbgPanic("This mode is unsupported");
    break;
  }
}

/**
 * Updates calibration data.
 */
void LSM303calibrator::update(const float *data, uint32_t still_msk){
  switch(state){
  case LSM303_CAL_SLEEP:
    /* nothing to do */
    return;
    break;

  case LSM303_CAL_WAIT_NEXT:
    if ((chTimeNow() - last_point_timestamp) < WAIT_NEXT_TIMEOUT){
      red_blink_mb.post((msg_t)slowblink, TIME_IMMEDIATE);
      return;
    }
    else{
      /* time is out */
      chSysLock();
      sample = 0;
      point = 0;
      state = LSM303_CAL_SLEEP;
      chSysUnlock();
      mavlink_dbg_print(MAV_SEVERITY_INFO, "MAG: wait next point time out");
      mavlink_system_struct.state = MAV_STATE_STANDBY;
    }
    break;

  case LSM303_CAL_COLLECTING:
    collecting(data, still_msk);
    break;

  default:
    chDbgPanic("unhandled case");
    break;
  }
}

/**
 * @brief   Guard code for @p CH_IRQ_EPILOGUE().
 *
 * @notapi
 */
void dbg_check_leave_isr(void) {

  port_lock_from_isr();
  if ((dbg_isr_cnt <= 0) || (dbg_lock_cnt != 0))
    chDbgPanic("SV#9");
  dbg_isr_cnt--;
  port_unlock_from_isr();
}

/**
 * @brief   Guard code for @p CH_IRQ_PROLOGUE().
 *
 * @notapi
 */
void dbg_check_enter_isr(void) {

  port_lock_from_isr();
  if ((dbg_isr_cnt < 0) || (dbg_lock_cnt != 0))
    chDbgPanic("SV#8");
  dbg_isr_cnt++;
  port_unlock_from_isr();
}

static msg_t PowerKeeperThread(void *arg){
  chRegSetThreadName("PowerKeeper");
  (void)arg;

  uint32_t batcap = 0;  /* battery capacitance in A*mS */
  uint32_t batfill = 0; /* battery filling in A*mS */
  int32_t i = -1;

  /* get current battery capacitance from parameter structure */
  i = _key_index_search("BAT_cap");
  if (i == -1)
    chDbgPanic("key not found");
  else
    batcap = 3600 * ((uint32_t)floorf(global_data[i].value));

  /* get battery fill in percents and calculate fill in A*mS*/
  i = _key_index_search("BAT_fill");
  if (i == -1)
    chDbgPanic("key not found");
  else
    batfill = (batcap * (uint32_t)floorf(global_data[i].value)) / 100;


  systime_t time = chTimeNow();     // T0
  while (TRUE) {
    time += MS2ST(PWR_CHECK_PERIOD);              // Next deadline

    raw_data.main_current = samples[ADC_CURRENT_SENS_OFFSET];
    raw_data.main_voltage = samples[ADC_MAIN_SUPPLY_OFFSET];
    raw_data.secondary_voltage = samples[ADC_6V_SUPPLY_OFFSET];

    comp_data.main_current = get_comp_main_current(raw_data.main_current);
    comp_data.secondary_voltage = get_comp_secondary_voltage(raw_data.secondary_voltage);

    batfill -= (comp_data.main_current * PWR_CHECK_PERIOD) / 1000;

    mavlink_sys_status_struct.battery_remaining = (batfill * 100) / batcap;
    mavlink_sys_status_struct.current_battery   = (uint16_t)(comp_data.main_current / 10);
    mavlink_sys_status_struct.voltage_battery   = comp_data.secondary_voltage;

    chThdSleepUntil(time);
  }
  return 0;
}

// todo: make this a macro? always inline?
efitime_t Overflow64Counter::get() {
#if EFI_PROD_CODE
	bool alreadyLocked = lockAnyContext();
	efitime_t localH = state.highBits;
	uint32_t localLow = state.lowBits;

	uint32_t value = GET_TIMESTAMP();

	if (value < localLow) {
		// new value less than previous value means there was an overflow in that 32 bit counter
		localH += 0x100000000LL;
	}

	efitime_t result = localH + value;

	if (!alreadyLocked) {
		unlockAnyContext();
	}
	return result;
#else

	/**
	 * this method is lock-free and thread-safe, that's because the 'update' method
	 * is atomic with a critical zone requirement.
	 *
	 * http://stackoverflow.com/questions/5162673/how-to-read-two-32bit-counters-as-a-64bit-integer-without-race-condition
	 */
	efitime_t localH;
	uint32_t localLow;
	int counter = 0;
	while (true) {
		localH = state.highBits;
		localLow = state.lowBits;
		efitime_t localH2 = state.highBits;
		if (localH == localH2)
			break;
#if EFI_PROD_CODE || defined(__DOXYGEN__)
		if (counter++ == 10000)
			chDbgPanic("lock-free frozen");
#endif /* EFI_PROD_CODE */
	}
	/**
	 * We need to take current counter after making a local 64 bit snapshot
	 */
	uint32_t value = GET_TIMESTAMP();

	if (value < localLow) {
		// new value less than previous value means there was an overflow in that 32 bit counter
		localH += 0x100000000LL;
	}

	return localH + value;
#endif
}

void __burn(BaseSequentialStream *chp, uint8_t pattern){
  volatile uint32_t status = 0;
  uint32_t eeprom_cycle;

  eeprom_cycle = BKP->DR1;
  eeprom_cycle = eeprom_cycle << 16;
  eeprom_cycle += BKP->DR2;

  if (BKP->DR3 == 1){
    //chprintf(chp, "EEPROM exhausted at %u try\n", eeprom_cycle);
    return;
  }

  /* fill memory buffer and write it to eeprom */
  memset(databuf, pattern, sizeof(databuf));
  chFileStreamSeek(&EepromFile, 0);
  status = chFileStreamWrite(&EepromFile, databuf, EEPROM_SIZE);
  if (status != sizeof(databuf))
    chDbgPanic("");

  /* read eeprom to test buffer */
  chFileStreamSeek(&EepromFile, 0);
  chFileStreamRead(&EepromFile, testbuf, EEPROM_SIZE);
  if (status != sizeof(databuf))
    chDbgPanic("");

  /* compare 2 buffers. Non zero value denotes bad sector in eeprom */
  status = memcmp(databuf, testbuf, EEPROM_SIZE);
  if (status != 0){
    BKP->DR3 = 1; /* bad sector(s) appeared */
    return;
  }

  /* Print note to console and increment cycle counter */
  //chprintf(chp, "EEPROM burn %u \n", eeprom_cycle);
  eeprom_cycle++;
  BKP->DR2 = eeprom_cycle & 0xFFFF;
  BKP->DR1 = (eeprom_cycle >> 16) & 0xFFFF;

  palTogglePad(IOPORT3, GPIOC_LED);
}

/**
 * Create telemetry link threads
 */
void SpawnMavlinkThreads(void *sdp) {

    linkout_tp = chThdCreateStatic(LinkOutThreadWA,
                                   sizeof(LinkOutThreadWA),
                                   LINK_THREADS_PRIO,
                                   LinkOutThread,
                                   sdp);

    if (linkout_tp == NULL)
        chDbgPanic("Can not allocate memory");

    linkin_tp = chThdCreateStatic(LinkInThreadWA,
                                  sizeof(LinkInThreadWA),
                                  LINK_THREADS_PRIO,
                                  LinkInThread,
                                  sdp);
    if (linkin_tp == NULL)
        chDbgPanic("Can not allocate memory");

    setGlobalFlag(GlobalFlags.tlm_link_ready);
}

Thread* eepromtest_clicmd(int argc, const char * const * argv, SerialDriver *sdp){
  (void)argc;
  (void)argv;

  Thread *eeprom_tp = chThdCreateFromHeap(&ThdHeap,
                                  sizeof(EepromTestThreadWA),
                                  NORMALPRIO,
                                  EepromTestThread,
                                  sdp);
  if (eeprom_tp == NULL)
    chDbgPanic("Can not allocate memory");
  return eeprom_tp;
}

Thread* CanInitLocal(void){
  canInit();
  canStart(&CAND1, &cancfg);

  can_tp = chThdCreateFromHeap(
      &ThdHeap,
      sizeof(CanTxThreadWA),
      LINK_THREADS_PRIO - 2,
      CanTxThread,
      NULL);

  if (can_tp == NULL)
    chDbgPanic("Can not allocate memory");

  return can_tp;
}

	void register_handler(const Message::ID id, MessageHandler&& handler) {
		if( map_[toUType(id)] != nullptr ) {
			chDbgPanic("MsgDblReg");
		}
		map_[toUType(id)] = std::move(handler);
	}

 void __cxa_pure_virtual() { chDbgPanic("Pure virtual function call."); }