void motor_driver_update_trajectory(motor_driver_t *d, trajectory_chunk_t *traj)
{
    chBSemWait(&d->lock);
    if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) {
        d->setpt.trajectory = chPoolAlloc(d->traj_buffer_pool);
        float *traj_mem = chPoolAlloc(d->traj_buffer_points_pool);
        if (d->setpt.trajectory == NULL || traj_mem == NULL) {
            chSysHalt("motor driver out of memory (trajectory buffer allocation)");
        }
        trajectory_init(d->setpt.trajectory, traj_mem, d->traj_buffer_nb_points, 4, traj->sampling_time_us);
        d->control_mode = MOTOR_CONTROL_MODE_TRAJECTORY;
    }
    int ret = trajectory_apply_chunk(d->setpt.trajectory, traj);
    switch (ret) {
        case TRAJECTORY_ERROR_TIMESTEP_MISMATCH:
            chSysHalt("TRAJECTORY_ERROR_TIMESTEP_MISMATCH");
            break;
        case TRAJECTORY_ERROR_CHUNK_TOO_OLD:
            chSysHalt("TRAJECTORY_ERROR_CHUNK_TOO_OLD");
            break;
        case TRAJECTORY_ERROR_DIMENSION_MISMATCH:
            chSysHalt("TRAJECTORY_ERROR_DIMENSION_MISMATCH");
            break;
        case TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER:
            log_message("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
            // chSysHalt("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER");
            break;
    }
    d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TRAJECTORY;
    chBSemSignal(&d->lock);
}

/**
 * @brief   Guard code for @p chSysUnlockFromIsr().
 *
 * @notapi
 */
void _dbg_check_unlock_from_isr(void) {

  if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) {
    chSysHalt("SV#7", __func__);
  }
  _dbg_leave_lock();
}

/*lint -save -e9075 [8.4] All symbols are invoked from asm context.*/
void _unhandled_exception(void) {
/*lint -restore*/

  chSysHalt("unhandled exception");
  while (true) {
  }
}

/*
 * DAC error callback.
 */
static void error_cb1(DACDriver *dacp, dacerror_t err) {

  (void)dacp;
  (void)err;

  chSysHalt("DAC failure");
}

/**
 * @brief   Guard code for @p chSysUnlock().
 *
 * @notapi
 */
void _dbg_check_unlock(void) {

  if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) {
    chSysHalt("SV#5");
  }
  _dbg_leave_lock();
}

/**
 * @brief   Guard code for @p chSysLock().
 *
 * @notapi
 */
void _dbg_check_lock(void) {

  if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
    chSysHalt("SV#4");
  }
  _dbg_enter_lock();
}

int _kill(int a, int b) {
	(void)a;
	(void)b;

	chSysHalt();
	return -1;
}

/* Called upon DAC error */
static void dac_error_callback(DACDriver* dacp, dacerror_t err)
{
    (void)dacp;
    (void)err;
    chSysHalt("Panic: DAC Error.");
    while(1);
}

/**
 * @brief   Guard code for @p chSysUnlockFromIsr().
 *
 * @notapi
 */
void _dbg_check_unlock_from_isr(void) {

  if ((nil.isr_cnt <= (cnt_t)0) || (nil.lock_cnt <= (cnt_t)0)) {
    chSysHalt("SV#7");
  }
  _dbg_leave_lock();
}

float motor_driver_get_position_setpt(motor_driver_t *d)
{
    if (d->control_mode != MOTOR_CONTROL_MODE_POSITION) {
        chSysHalt("motor driver get position wrong setpt mode");
    }
    return d->setpt.position;
}

/**
 * @brief   Guard code for @p chSysLockFromIsr().
 *
 * @notapi
 */
void _dbg_check_lock_from_isr(void) {

  if ((nil.isr_cnt <= (cnt_t)0) || (nil.lock_cnt != (cnt_t)0)) {
    chSysHalt("SV#6");
  }
  _dbg_enter_lock();
}

/**
 * @brief   Low level HAL driver initialization.
 *
 * @notapi
 */
void hal_lld_init(void) {
  extern void _vectors(void);
  uint32_t reg;

  /* The system is switched to the RUN0 mode, the default for normal
     operations.*/
  if (halSPCSetRunMode(SPC5_RUNMODE_RUN0) == CH_FAILED)
    chSysHalt();

  /* INTC initialization, software vector mode, 4 bytes vectors, starting
     at priority 0.*/
  INTC.MCR.R        = 0;
  INTC.CPR.R        = 0;
  INTC.IACKR.R      = (uint32_t)_vectors;

  /* PIT channel 0 initialization for Kernel ticks, the PIT is configured
     to run in DRUN,RUN0...RUN3 and HALT0 modes, the clock is gated in other
     modes.*/
  INTC.PSR[59].R    = SPC5_PIT0_IRQ_PRIORITY;
  halSPCSetPeripheralClockMode(92,
                               SPC5_ME_PCTL_RUN(2) | SPC5_ME_PCTL_LP(2));
  reg = halSPCGetSystemClock() / CH_FREQUENCY - 1;
  PIT.PITMCR.R      = 1;        /* PIT clock enabled, stop while debugging. */
  PIT.CH[0].LDVAL.R = reg;
  PIT.CH[0].CVAL.R  = reg;
  PIT.CH[0].TFLG.R  = 1;        /* Interrupt flag cleared.                  */
  PIT.CH[0].TCTRL.R = 3;        /* Timer active, interrupt enabled.         */
}

/**
 * @brief   Guard code for @p chSysUnlockFromIsr().
 *
 * @notapi
 */
void _dbg_check_unlock_from_isr(void) {

  if ((ch.dbg.isr_cnt <= 0) || (ch.dbg.lock_cnt <= 0)) {
    chSysHalt("SV#7");
  }
  _dbg_leave_lock();
}

/**
 * @brief   Guard code for @p chSysLockFromIsr().
 *
 * @notapi
 */
void _dbg_check_lock_from_isr(void) {

  if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) {
    chSysHalt("SV#6", __func__);
  }
  _dbg_enter_lock();
}

/*
 * Application entry point.
 */
int main(void) {
  msg_t status = RDY_TIMEOUT;

  halInit();
  chSysInit();
  chBSemInit(&alarm_sem, TRUE);

  rtcGetTime(&RTCD1, &timespec);
  alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
  rtcSetAlarm(&RTCD1, 0, &alarmspec);

  rtcSetCallback(&RTCD1, rtc_cb);
  while (TRUE) {

    /* Wait until alarm callback signaled semaphore.*/
    status = chBSemWaitTimeout(&alarm_sem, S2ST(RTC_ALARMPERIOD + 10));

    if (status == RDY_TIMEOUT) {
      chSysHalt();
    }
    else {
      rtcGetTime(&RTCD1, &timespec);
      alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
      rtcSetAlarm(&RTCD1, 0, &alarmspec);
    }
  }
  return 0;
}

void motor_driver_get_trajectory_point(motor_driver_t *d,
                                       int64_t timestamp_us,
                                       float *position,
                                       float *velocity,
                                       float *acceleration,
                                       float *torque)
{
    if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) {
        chSysHalt("motor driver get trajectory wrong setpt mode");
    }
    float *t = trajectory_read(d->setpt.trajectory, timestamp_us);
    if (t == NULL) {
        // chSysHalt("control error"); // todo
        log_message("trajectory read: %d failed", timestamp_get());
        *position = 0;
        *velocity = 0;
        *acceleration = 0;
        *torque = 0;
        return;
    }
    *position = t[0];
    *velocity = t[1];
    *acceleration = t[2];
    *torque = t[3];
}

void message_server_thread(void *arg)
{
    struct netconn *conn;
    struct netbuf *buf;
    static uint8_t buffer[4096];
    err_t err;
    LWIP_UNUSED_ARG(arg);

    chRegSetThreadName("rpc_message");

    conn = netconn_new(NETCONN_UDP);
    if (conn == NULL) {
        chSysHalt("Cannot create SimpleRPC message server connection (out of memory).");
    }
    netconn_bind(conn, NULL, MSG_SERVER_PORT);

    while (1) {
        err = netconn_recv(conn, &buf);

        if (err == ERR_OK) {
            netbuf_copy(buf, buffer, buf->p->tot_len);
            message_process(buffer, buf->p->tot_len, message_callbacks, message_callbacks_len);
        }
        netbuf_delete(buf);
    }
}

float motor_driver_get_voltage_setpt(motor_driver_t *d)
{
    if (d->control_mode != MOTOR_CONTROL_MODE_VOLTAGE) {
        chSysHalt("motor driver get voltage wrong setpt mode");
    }
    return d->setpt.voltage;
}

float motor_driver_get_torque_setpt(motor_driver_t *d)
{
    if (d->control_mode != MOTOR_CONTROL_MODE_TORQUE) {
        chSysHalt("motor driver get torque wrong setpt mode");
    }
    return d->setpt.torque;
}

float motor_driver_get_velocity_setpt(motor_driver_t *d)
{
    if (d->control_mode != MOTOR_CONTROL_MODE_VELOCITY) {
        chSysHalt("motor driver get velocity wrong setpt mode");
    }
    return d->setpt.velocity;
}