void
app_cfg_idle()
{
  if ((chTimeNow() - last_idle) > S2ST(2)) {
    app_cfg_flush();
    last_idle = chTimeNow();
  }
}

/**
 * @brief   Current timestamp in milliseconds.
 * @note    The resolution is in milliseconds, but the precision may not be.
 *
 * @return
 *          The current timestamp, with a resolution of one millisecond.
 */
uint32_t uros_lld_threading_gettimestampmsec(void) {

#if CH_FREQUENCY == 1000
  return (uint32_t)chTimeNow();
#else
  return (((uint32_t)chTimeNow() - 1) * 1000) / CH_FREQUENCY + 1;
#endif
}

void SpeedMetersConfig(SpeedMeter_callback_t _left_cb, SpeedMeter_callback_t _right_cb){
  left_cb = _left_cb;
  right_cb = _right_cb;
     
  extStart(&EXTD1, &extcfg);
  
  tl1 = tl2 = chTimeNow();
  tr1 = tr2 = chTimeNow();
}

void Keys_t::ITask() {
    chThdSleepMilliseconds(KEYS_POLL_PERIOD_MS);
    if(App.PThd == nullptr) return;
    // Check keys
    for(uint8_t i=0; i<KEYS_CNT; i++) {
        bool PressedNow = !PinIsSet(KeyData[i].PGpio, KeyData[i].Pin);
        // Check if just pressed
        if(PressedNow and !Key[i].IsPressed) {
            chSysLock();
            Key[i].IsPressed = true;
            Key[i].IsLongPress = false;
            Key[i].IsRepeating = false;
            chEvtSignalI(App.PThd, KeyData[i].EvtMskPress);
            // Reset timers
            RepeatTimer = chTimeNow();
            LongPressTimer = chTimeNow();
            chSysUnlock();
        }
        // Check if just released
        else if(!PressedNow and Key[i].IsPressed) {
            Key[i].IsPressed = false;
            if(!Key[i].IsLongPress) {
                chSysLock();
                chEvtSignalI(App.PThd, KeyData[i].EvtMskRelease);
                chSysUnlock();
            }
        }
        // Check if still pressed
        else if(PressedNow and Key[i].IsPressed) {
            // Check if long press
            if(!Key[i].IsLongPress) {
                if(TimeElapsed(&LongPressTimer, KEY_LONGPRESS_DELAY_MS)) {
                    Key[i].IsLongPress = true;
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskLongPress);
                    chSysUnlock();
                }
            }
            // Check if repeat
            if(!Key[i].IsRepeating) {
                if(TimeElapsed(&RepeatTimer, KEYS_KEY_BEFORE_REPEAT_DELAY_MS)) {
                    Key[i].IsRepeating = true;
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskRepeat);
                    chSysUnlock();
                }
            }
            else {
                if(TimeElapsed(&RepeatTimer, KEY_REPEAT_PERIOD_MS)) {
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskRepeat);
                    chSysUnlock();
                }
            }
        } // if still pressed
    } // for
}

static state_t do_state_pad(instance_data_t *data)
{
    state_estimation_trust_barometer = true;
    if(chTimeNow() > 10000 && data->state.a > IGNITION_ACCEL) {
        data->t_launch = chTimeNow();
        return STATE_IGNITION;
    } else {
        return STATE_PAD;
    }
}

RadInputValue inputEncoderProcessor(RadInputState* state)
{
  RadInputValue v;
  v.encoder.delta = state->encoder.value - state->encoder.last_value;
  state->encoder.last_value = state->encoder.value;
  v.encoder.rate =
      (float)v.encoder.delta /
      (chTimeNow() - state->encoder.last_time) * S2ST(1);
  state->encoder.last_time = chTimeNow();
  return v;
}

u32_t sys_now(void) {

#if CH_FREQUENCY == 1000
  return (u32_t)chTimeNow();
#elif (CH_FREQUENCY / 1000) >= 1 && (CH_FREQUENCY % 1000) == 0
  return ((u32_t)chTimeNow() - 1) / (CH_FREQUENCY / 1000) + 1;
#elif (1000 / CH_FREQUENCY) >= 1 && (1000 % CH_FREQUENCY) == 0
  return ((u32_t)chTimeNow() - 1) * (1000 / CH_FREQUENCY) + 1;
#else
  return (u32_t)(((u64_t)(chTimeNow() - 1) * 1000) / CH_FREQUENCY) + 1;
#endif
}

int advp_send(uint8_t *frame, d7_ti time, uint8_t bg_channel, uint8_t fg_channel)
{
    systime_t now;
    systime_t end;
    uint8_t bg_frame[FG_FRAME_SIZE];
    bg_param_t conf;
    int error;
    int step = 0;

    //min advp time is needed because the first send takes some time
    if(time < MIN_ADVP_TIME) time = MIN_ADVP_TIME;

    //configure the background send procedure
    bg_set_subnet(bg_frame, data_proc_conf.my_subnet);
    conf.channel = fg_channel;

    data_proc_conf.pack_class = BACKGROUND_CLASS;
    data_proc_conf.channel = bg_channel;
    data_proc_config(&data_proc_conf);

    now = chTimeNow();
    end = now + d7_ti_to_systime(time);

    //keep sending packets with the remaining time
    while(now < (end - (PROT_SEND_TIME)))
    {
        conf.time = systime_to_d7_ti(end - now - (PROT_SEND_TIME - SAFE_ADVP_WINDOW));
        bg_set_advp(bg_frame,&conf);
        data_add_frame(bg_frame);
        if(!step){
            error = data_send_packet();
            step = 1;
        } else
            error = data_send_packet_protected();
        if(error){
        }

        now = chTimeNow();
    }

    //prepare the foreground send procedure
    data_proc_conf.pack_class = FOREGROUND_CLASS;
    data_proc_conf.channel = fg_channel;
    data_proc_config(&data_proc_conf);
    data_add_frame(frame);
    now = chTimeNow();
    //if(now > end) 
    //    return -(now - end);
    //chThdSleepMilliseconds(end-now);

    return data_send_packet_protected();
}

void writeToFlash(void) {
    flashState.version = FLASH_DATA_VERSION;
    scheduleMsg(&logger, "FLASH_DATA_VERSION=%d", flashState.version);
    crc result = flashStateCrc(&flashState);
    flashState.value = result;
    scheduleMsg(&logger, "Reseting flash=%d", FLASH_USAGE);
    flashErase(FLASH_ADDR, FLASH_USAGE);
    scheduleMsg(&logger, "Flashing with CRC=%d", result);
    time_t now = chTimeNow();
    result = flashWrite(FLASH_ADDR, (const char *) &flashState, FLASH_USAGE);
    scheduleMsg(&logger, "Flash programmed in (ms): %d", chTimeNow() - now);
    scheduleMsg(&logger, "Flashed: %d", result);
}

u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
  systime_t time, tmo;

  chSysLock();
  tmo = timeout > 0 ? (systime_t)timeout : TIME_INFINITE;
  time = chTimeNow();
  if (chMBFetchS(*mbox, (msg_t *)msg, tmo) != RDY_OK)
    time = SYS_ARCH_TIMEOUT;
  else
    time = chTimeNow() - time;
  chSysUnlock();
  return time;
}

u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
  systime_t time, tmo;

  chSysLock();
  tmo = timeout > 0 ? (systime_t)timeout : TIME_INFINITE;
  time = chTimeNow();
  if (chSemWaitTimeoutS(*sem, tmo) != RDY_OK)
    time = SYS_ARCH_TIMEOUT;
  else
    time = chTimeNow() - time;
  chSysUnlock();
  return time;
}

void inputGinputFetcher(RadInputConfig* config, RadInputState* state)
{
  if (state->is_enabled == 1) {
    // Initialization
    ginputGetMouse(0);
    state->is_enabled = 2;
    state->encoder.last_time = chTimeNow();
  }

  ginputGetMouseStatus(0, &config->ginput.event);

  uint8_t is_down = (config->ginput.event.current_buttons & config->ginput.button_mask) ? 1 : 0;
  state->encoder.value = config->ginput.event.x / 2;
  if (abs((int)state->encoder.value - state->encoder.last_value) > 50)
    state->encoder.last_value = state->encoder.value;

  // Modifying the state of the next channel: button channel
  RadInputState* state_button = state + 1;
  if (is_down)
  {
    if (!state_button->button.is_down) {
      state_button->button.is_down = 1;
      state_button->button.times++;
    }
  } else {
    state_button->button.is_down = 0;
  }
}

msg_t qeipub_node(void *arg) {
	Node node("qeipub");
	Publisher<tQEIMsg> qei_pub;
	systime_t time;
	tQEIMsg *msgp;

	(void) arg;
	chRegSetThreadName("qeipub");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(qei_pub, "qei"MOTOR_ID_STRING);

	for (;;) {
		time = chTimeNow();
		int16_t delta = qeiUpdate(&QEI_DRIVER);

		if (qei_pub.alloc(msgp)) {
			msgp->timestamp.sec = 0;
			msgp->timestamp.nsec = 0;
			msgp->delta = delta;
			qei_pub.publish(*msgp);
		}

		time += MS2ST(50);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}

bool LSM303calibrator::trig(void){
  switch(state){
  case LSM303_CAL_SLEEP:

    chSysLock();
    last_point_timestamp = chTimeNow();
    sample = 0;
    point = 0;
    state = LSM303_CAL_COLLECTING;
    chSysUnlock();

    mavlink_dbg_print(MAV_SEVERITY_INFO, "MAG: calibration started");
    chThdSleep(1);
    mavlink_system_struct.state = MAV_STATE_CALIBRATING;
    return CH_SUCCESS;
    break;

  case LSM303_CAL_WAIT_NEXT:
    mavlink_dbg_print(MAV_SEVERITY_NOTICE, "MAG: new position reached");
    state = LSM303_CAL_COLLECTING;
    return CH_SUCCESS;
    break;

  default:
    return CH_FAILED;
    break;
  }
}

int main(void)
{
	halInit();
	chSysInit();

	mcu_conf();

	for (int i = 0; i < 10; i++)
	{
		chThdSleepMilliseconds(100);
		palTogglePad(TEST_LED_PORT3, TEST_LED_PIN3);
	}

	ap.start();
	appInit();

	ph.StartAutoIdle();
	ph.setFunctionTable(&ph_ft);
	enable_watchdog();
	ph.RequestData(STARTUP);

	while (TRUE)
	{
		Scheduler::Play();
		sysTime = chTimeNow();
		ph.HandlePacketLoop();
	}

	return 1;
}

static void cmd_rms(BaseSequentialStream *chp) {

  /* 
   * Creating dynamic threads using the heap allocator
  */
  Thread *tp1 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-2, thread1, chp);
  Thread *tp2 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO, thread2, chp);
  Thread *tp3 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-1, thread3, chp);
  Thread *tp4 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-3, thread4, chp);


  chThdSleepUntil(chTimeNow() + MS2ST(500));

  /*
   * Try to kill threads
  */
  chThdTerminate(tp1);
  chThdTerminate(tp2);
  chThdTerminate(tp3);
  chThdTerminate(tp4);

  /*
   * Wait for the thread to terminate (if it has not terminated
   * already) then get the thread exit message (msg) and returns the
   * terminated thread memory to the heap.
   */
  msg_t msg = chThdWait(tp1);
  msg = chThdWait(tp2);
  msg = chThdWait(tp3);
  msg = chThdWait(tp4);
}

// Implements steps according to the current step interval
// You must call this at least once per step
// returns true if a step occurred
bool AccelStepper::runSpeed()
{
    // Dont do anything unless we actually have a step interval
    if (!_stepInterval)
	return false;

    //unsigned long time = micros();
    unsigned long time = chTimeNow();
    // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
    unsigned long nextStepTime = _lastStepTime + _stepInterval;
    if (   ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
	|| ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime))))

    {
	if (_direction == DIRECTION_CW)
	{
	    // Clockwise
	    _currentPos += 1;
	}
	else
	{
	    // Anticlockwise  
	    _currentPos -= 1;
	}
	step(_currentPos & 0x7); // Bottom 3 bits (same as mod 8, but works with + and - numbers) 

	_lastStepTime = time;
	return true;
    }
    else
    {
	return false;
    }
}

uint32_t get_timestamp(void) {
    systime_t new_stime;
    new_stime = chTimeNow();
    last_timestamp += (((new_stime-last_systime) * 1000) / CH_FREQUENCY);
    last_systime = new_stime;
    return last_timestamp;
}

static msg_t uart_thread(void *arg) {
	(void)arg;

	chRegSetThreadName("UART");

	uartStart(&HW_UART_DEV, &uart_cfg);
	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);
	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);

	systime_t time = chTimeNow();

	for(;;) {
		time += MS2ST(1);

		if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time)
				/ ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) {
			mcpwm_set_brake_current(-10.0);
		} else {
			set_output(out_received);
		}

		chThdSleepUntil(time);
	}

	return 0;
}

static msg_t
VexSonarTask( void *arg )
{
    tVexSonarChannel    c;

    (void)arg;

    chRegSetThreadName("sonar");

    gptStart( sonarGpt, &vexSonarGpt );

    while(!chThdShouldTerminate())
        {
        if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
            {
            // ping sonar
            vexSonarPing(nextSonar);

            // wait for next time slot
            // the timer is set to timeout in 40mS but we need a 10mS gap before any more
            // pings can be sent
            chThdSleepUntil(chTimeNow() + 50);

            // calculate echo time
            vexSonars[nextSonar].time = vexSonars[nextSonar].time_f - vexSonars[nextSonar].time_r;

            // was the time too great ?
            if( vexSonars[nextSonar].time > 35000 )
                vexSonars[nextSonar].time = -1;

            // if we have a valid time calculate real distance
            if( vexSonars[nextSonar].time != -1 )
                {
                vexSonars[nextSonar].distance_cm = vexSonars[nextSonar].time   / 58;
                vexSonars[nextSonar].distance_inch = vexSonars[nextSonar].time / 148;
                }
            else
                {
                vexSonars[nextSonar].distance_cm = -1;
                vexSonars[nextSonar].distance_inch = -1;
                }

            // look for next sonar
            for(c=kVexSonar_1;c<kVexSonar_Num;c++)
                {
                if( ++nextSonar == kVexSonar_Num )
                    nextSonar = kVexSonar_1;

                // we need sonar to be installed and enabled
                if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
                    break;
                }
            }
        else
            // Nothing enabled, just wait
            chThdSleepMilliseconds(25);
        }

    return (msg_t)0;
}