bool callback(const PWM2Msg &msg) {
	int16_t pwm;

	pwm = msg.value[MOTOR_ID];

	chSysLock();

	if (pwm > 0) {
		pwm_lld_enable_channel(&PWM_DRIVER, 0, pwm);
		pwm_lld_enable_channel(&PWM_DRIVER, 1, 0);
	} else {
		pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
		pwm_lld_enable_channel(&PWM_DRIVER, 1, -pwm);
	}

	chSysUnlock();

	return true;
}

bool enc_callback(const r2p::tEncoderMsg &msg) {

	pwm = speed_pid.update(msg.delta);

	chSysLock();

	if (pwm > 0) {
		pwm_lld_enable_channel(&PWM_DRIVER, 1, pwm);
		pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
	} else {
		pwm_lld_enable_channel(&PWM_DRIVER, 1, 0);
		pwm_lld_enable_channel(&PWM_DRIVER, 0, -pwm);
	}
	chSysUnlock();

	palTogglePad(LED2_GPIO, LED2);

	return true;
}

/**
 * @brief   Changes the running thread priority level then reschedules if
 *          necessary.
 * @note    The function returns the real thread priority regardless of the
 *          current priority that could be higher than the real priority
 *          because the priority inheritance mechanism.
 *
 * @param[in] newprio   the new priority level of the running thread
 * @return              The old priority level.
 *
 * @api
 */
tprio_t chThdSetPriority(tprio_t newprio) {
  tprio_t oldprio;

  chDbgCheck(newprio <= HIGHPRIO, "chThdSetPriority");

  chSysLock();
#if CH_USE_MUTEXES
  oldprio = currp->p_realprio;
  if ((currp->p_prio == currp->p_realprio) || (newprio > currp->p_prio))
    currp->p_prio = newprio;
  currp->p_realprio = newprio;
#else
  oldprio = currp->p_prio;
  currp->p_prio = newprio;
#endif
  chSchRescheduleS();
  chSysUnlock();
  return oldprio;
}

void l3g4200d_update(SPIDriver *spip) {
  int16_t data[3];

  gyro_data.t = chTimeNow();

  //XXX da fare lettura sequenziale!
  data[0] = (l3g4200dReadRegister(spip, L3G4200D_OUT_X_H) & 0xFF) << 8;
  data[0] |= l3g4200dReadRegister(spip, L3G4200D_OUT_X_L) & 0xFF;
  data[1] = (l3g4200dReadRegister(spip, L3G4200D_OUT_Y_H) & 0xFF) << 8;
  data[1] |= l3g4200dReadRegister(spip, L3G4200D_OUT_Y_L) & 0xFF;
  data[2] = (l3g4200dReadRegister(spip, L3G4200D_OUT_Z_H) & 0xFF) << 8;
  data[2] |= l3g4200dReadRegister(spip, L3G4200D_OUT_Z_L) & 0xFF;

  chSysLock();
  gyro_data.x = data[0];
  gyro_data.y = data[1];
  gyro_data.z = data[2];
  chSysUnlock();
}

/**
 * @brief   Transmits data via the I2C bus as master.
 * @details Number of receiving bytes must be 0 or more than 1 on STM32F1x.
 *          This is hardware restriction.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 * @retval RDY_TIMEOUT  if a timeout occurred before operation end. <b>After a
 *                      timeout the driver must be stopped and restarted
 *                      because the bus is in an uncertain state</b>.
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
  LPC_I2C_TypeDef *dp = i2cp->i2c;
  VirtualTimer vt;

  i2cp->addr = addr << 1;
  /* Global timeout for the whole operation.*/
  if (timeout != TIME_INFINITE)
    chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);

  /* Releases the lock from high level driver.*/
  chSysUnlock();

  /* Initializes driver fields */
  i2cp->errors = 0;
  i2cp->txbuf = txbuf;
  i2cp->txbytes = txbytes;
  i2cp->rxbuf = rxbuf;
  i2cp->rxbytes = rxbytes;

  /* This lock will be released in high level driver.*/
  chSysLock();

  /* Atomic check on the timer in order to make sure that a timeout didn't
     happen outside the critical zone.*/
  if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
    return RDY_TIMEOUT;

  /* Starts the operation.*/
  dp->CONSET = I2C_CONSET_STA;

  /* Waits for the operation completion or a timeout.*/
  i2cp->thread = chThdSelf();
  chSchGoSleepS(THD_STATE_SUSPENDED);

  if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
    chVTResetI(&vt);

  return chThdSelf()->p_u.rdymsg;
}

uint8_t i2c_t::WriteWrite(uint32_t Addr, uint8_t *WPtr1, uint32_t WLength1, uint8_t *WPtr2, uint32_t WLength2) {
    if(chBSemWait(&BSemaphore) != MSG_OK) return FAILURE;
    uint8_t Rslt;
    msg_t r;
    I2C_TypeDef *pi2c = PParams->pi2c;  // To make things shorter
    if(WLength1 == 0 or WPtr1 == nullptr) { Rslt = CMD_ERROR; goto WriteWriteEnd; }
    if(IBusyWait() != OK) { Rslt = BUSY; goto WriteWriteEnd; }
    IReset(); // Reset I2C
    // Prepare TX DMA
    dmaStreamSetMode(PParams->PDmaTx, DMA_MODE_TX);
    dmaStreamSetMemory0(PParams->PDmaTx, WPtr1);
    dmaStreamSetTransactionSize(PParams->PDmaTx, WLength1);
    // Prepare transmission
    if(WLength2 != 0 and WPtr2 != nullptr) {
        IState = istWriteWrite;
        IPtr = WPtr2;
        ILen = WLength2;
        pi2c->CR2 = (Addr << 1) | (WLength1 << 16) | I2C_CR2_RELOAD;
    }
    else { // No second write
        IState = istWrite;
        pi2c->CR2 = (Addr << 1) | (WLength1 << 16);
    }
    dmaStreamEnable(PParams->PDmaTx);   // Enable TX DMA
    // Enable IRQs: TX completed, error, NAck
    pi2c->CR1 |= (I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE);
    pi2c->CR2 |= I2C_CR2_START;         // Start transmission
    // Wait completion
    chSysLock();
    r = chThdSuspendTimeoutS(&PThd, MS2ST(I2C_TIMEOUT_MS));
    chSysUnlock();
    // Disable IRQs
    pi2c->CR1 &= ~(I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE);
    if(r == MSG_TIMEOUT) {
        pi2c->CR2 |= I2C_CR2_STOP;
        Rslt = TIMEOUT;
    }
    else Rslt = (IState == istFailure)? FAILURE : OK;
    WriteWriteEnd:
    chBSemSignal(&BSemaphore);
    return Rslt;
}

static void writeOscQueue( InputQueue * q )
{
	msg_t msg;
	uint8_t noData;
	size_t cnt, i;
	chSysLock();
		cnt = (chQSpaceI( q ) / 2) * 2;
	chSysUnlock();
	// And just limit send data size to ensure USB chip will not overfull.
	cnt = ( cnt <= USB_FIFO_SZ ) ? cnt : USB_FIFO_SZ;
	for ( i=0; i<cnt; i++ )
	{
		msg = chIQGetTimeout( q, TIME_IMMEDIATE );
		noData = ( ( msg == Q_TIMEOUT ) || ( msg == Q_RESET ) ) ? 1 : 0;
		uint8_t v;
		v = ( noData ) ? 0 : (uint8_t)msg;
		writeResult( v );
	}
	writeEom();
}

/**
 * @brief   Returns the thread next to the specified one.
 * @details The reference counter of the specified thread is decremented and
 *          the reference counter of the returned thread is incremented.
 *
 * @param[in] tp        pointer to the thread
 * @return              A reference to the next thread.
 * @retval NULL         if there is no next thread.
 *
 * @api
 */
Thread *chRegNextThread(Thread *tp) {
  Thread *ntp;

  chSysLock();
  ntp = tp->p_newer;
  if (ntp == (Thread *)&rlist)
    ntp = NULL;
#if CH_USE_DYNAMIC
  else {
    chDbgAssert(ntp->p_refs < 255, "chRegNextThread(), #1",
                "too many references");
    ntp->p_refs++;
  }
#endif
  chSysUnlock();
#if CH_USE_DYNAMIC
  chThdRelease(tp);
#endif
  return ntp;
}

/** button interupt universal **/
void buttonInterupt(){
    static virtual_timer_t vt4;

    chSysLock();
        thread_t *ntp = tp_button;
        if(ntp){
            tp_button = NULL;
            chSchWakeupS(ntp, MSG_OK);
        }
        chSysUnlock();

  palSetPad(GPIOD, GPIOD_LED6);

  chSysLockFromISR();
  chVTResetI(&vt4);

  /* LED4 set to OFF after 200mS.*/
  chVTSetI(&vt4, MS2ST(200), led5off, NULL);
  chSysUnlockFromISR();
}

/**
 * @brief   Unregisters an Event Listener from its Event Source.
 * @note    If the event listener is not registered on the specified event
 *          source then the function does nothing.
 * @note    For optimal performance it is better to perform the unregister
 *          operations in inverse order of the register operations (elements
 *          are found on top of the list).
 *
 * @param[in] esp       pointer to the  @p event_source_t structure
 * @param[in] elp       pointer to the @p event_listener_t structure
 *
 * @api
 */
void chEvtUnregister(event_source_t *esp, event_listener_t *elp) {
  event_listener_t *p;

  chDbgCheck((esp != NULL) && (elp != NULL));

  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  p = (event_listener_t *)esp;
  /*lint -restore*/
  chSysLock();
  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  while (p->next != (event_listener_t *)esp) {
  /*lint -restore*/
    if (p->next == elp) {
      p->next = elp->next;
      break;
    }
    p = p->next;
  }
  chSysUnlock();
}

void RemotePublisher::subscribe(LocalSubscriber * sub, void * buffer,
		size_t size) {

	chPoolLoadArray(&_pool, buffer, size);

	chSysLock();
	if (this->_subscribers == NULL) {
		this->_subscribers = sub;
	} else {
		sub->link(_subscribers);
		_subscribers = sub;
	}

	_rtcan_msg.data = (uint8_t *) allocI();
	if (_rtcan_msg.data) {
		rtcanRegister(&RTCAND, &_rtcan_msg);
	}

	chSysUnlock();
}

msg_t pwm_node(void * arg) {
	uint8_t index = *(reinterpret_cast<uint8_t *>(arg));
	r2p::Node node("pwm2sub");
	r2p::Subscriber<r2p::PWM2Msg, 5> pwm_sub;
	r2p::PWM2Msg * msgp;

	(void) arg;

	chRegSetThreadName("pwm_node");

	/* Enable the h-bridge. */
	palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
	chThdSleepMilliseconds(500);
	pwmStart(&PWM_DRIVER, &pwmcfg);

	node.subscribe(pwm_sub, "pwm");

	for (;;) {
		if (node.spin(r2p::Time::ms(1000))) {
			if (pwm_sub.fetch(msgp)) {
				pwm = msgp->value[index];
				chSysLock()
				;
				if (pwm >= 0) {
					pwm_lld_enable_channel(&PWMD1, 0, msgp->value[index]);
					pwm_lld_enable_channel(&PWMD1, 1, 0);
				} else {
					pwm_lld_enable_channel(&PWMD1, 0, 0);
					pwm_lld_enable_channel(&PWMD1, 1, -msgp->value[index]);
				}
				chSysUnlock();
				pwm_sub.release(*msgp);
			}
		} else {
			// Stop motor if no messages for 1000 ms
			pwm_lld_disable_channel(&PWM_DRIVER, 0);
			pwm_lld_disable_channel(&PWM_DRIVER, 1);
		}
	}
	return CH_SUCCESS;
}

/**
 * @brief   Transmits data via the I2C bus as master.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 * @retval RDY_TIMEOUT  if a timeout occurred before operation end. <b>After a
 *                      timeout the driver must be stopped and restarted
 *                      because the bus is in an uncertain state</b>.
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
    i2cp->addr = addr;
    i2cp->txbuf = txbuf;
    i2cp->txbytes = txbytes;
    i2cp->txidx = 0;
    i2cp->rxbuf = rxbuf;
    i2cp->rxbytes = rxbytes;
    i2cp->rxidx = 0;

    TWCR = ((1 << TWSTA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE));

    chSysLock();
    i2cp->thread = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    chSysUnlock();

    return chThdSelf()->p_u.rdymsg;
}

//-----------------------------------------------------------------------------
int
kuroBoxWriterStop(void)
{
	chThdTerminate(loggerThread);
	chThdTerminate(writerThread);

	chThdWait(loggerThread);
	// this thread may be in its own sleep
	chSysLock();
	if (writerThreadForSleep)
	{
		writerThreadForSleep->p_u.rdymsg = (msg_t)10; // just a random non-0
		chSchReadyI(writerThreadForSleep);
		writerThreadForSleep = NULL;
	}
	chSysUnlock();

	chThdWait(writerThread);

	return KB_OK;
}

/**
 * @brief   Blocks the execution of the invoking thread until the specified
 *          thread terminates then the exit code is returned.
 * @details This function waits for the specified thread to terminate then
 *          decrements its reference counter, if the counter reaches zero then
 *          the thread working area is returned to the proper allocator.<br>
 *          The memory used by the exited thread is handled in different ways
 *          depending on the API that spawned the thread:
 *          - If the thread was spawned by @p chThdCreateStatic() or by
 *            @p chThdInit() then nothing happens and the thread working area
 *            is not released or modified in any way. This is the default,
 *            totally static, behavior.
 *          - If the thread was spawned by @p chThdCreateFromHeap() then
 *            the working area is returned to the system heap.
 *          - If the thread was spawned by @p chThdCreateFromMemoryPool()
 *            then the working area is returned to the owning memory pool.
 *          .
 * @pre     The configuration option @p CH_USE_WAITEXIT must be enabled in
 *          order to use this function.
 * @post    Enabling @p chThdWait() requires 2-4 (depending on the
 *          architecture) extra bytes in the @p Thread structure.
 * @post    After invoking @p chThdWait() the thread pointer becomes invalid
 *          and must not be used as parameter for further system calls.
 * @note    If @p CH_USE_DYNAMIC is not specified this function just waits for
 *          the thread termination, no memory allocators are involved.
 *
 * @param[in] tp        pointer to the thread
 * @return              The exit code from the terminated thread.
 *
 * @api
 */
msg_t chThdWait(Thread *tp) {
  msg_t msg;

  chDbgCheck(tp != NULL, "chThdWait");

  chSysLock();
  chDbgAssert(tp != currp, "chThdWait(), #1", "waiting self");
#if CH_USE_DYNAMIC
  chDbgAssert(tp->p_refs > 0, "chThdWait(), #2", "not referenced");
#endif
  if (tp->p_state != THD_STATE_FINAL) {
    list_insert(currp, &tp->p_waiting);
    chSchGoSleepS(THD_STATE_WTEXIT);
  }
  msg = tp->p_u.exitcode;
  chSysUnlock();
#if CH_USE_DYNAMIC
  chThdRelease(tp);
#endif
  return msg;
}

static void rt_test_005_004_execute(void) {

  /* [5.4.1] A thread is created, it goes to wait on the semaphore.*/
  test_set_step(1);
  {
    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread1, "A");
  }

  /* [5.4.2] The semaphore counter is increased by two, it is then
     tested to be one, the thread must have completed.*/
  test_set_step(2);
  {
    chSysLock();
    chSemAddCounterI(&sem1, 2);
    chSchRescheduleS();
    chSysUnlock();
    test_wait_threads();
    test_assert_lock(chSemGetCounterI(&sem1) == 1, "invalid counter");
    test_assert_sequence("A", "invalid sequence");
  }
}

/**
 * @brief Shell termination handler.
 *
 * @param[in] id event id.
 */
static void termination_handler(eventid_t id) {

	chThdSleepMilliseconds(10);

	cputs("Init: shell on SD1 terminated");
	chSysLock();
	chOQResetI(&SD1.oqueue);
	chSysUnlock();

	// todo: 2nd port for TS

//  if (shelltp2 && chThdTerminated(shelltp2)) {
//    chThdWait(shelltp2);
//    shelltp2 = NULL;
//    chThdSleepMilliseconds(10);
//    cputs("Init: shell on SD2 terminated");
//    chSysLock();
//    chOQResetI(&SD2.oqueue);
//    chSysUnlock();
//  }
}

static msg_t l3g4200d_update_thread(void *p) {
  SPIDriver *spip = (SPIDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/
    chSysLock();
    gyrotp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == GYRO_DATA_READY) {
      l3g4200d_update(spip);
    }
  }

  return RDY_OK;
}

/**
 * @brief   Stops an ongoing conversion.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 */
void adcStopConversion(ADCDriver *adcp) {

    chDbgCheck(adcp != NULL, "adcStopConversion");

    chSysLock();
    chDbgAssert((adcp->ad_state == ADC_READY) ||
                (adcp->ad_state == ADC_RUNNING) ||
                (adcp->ad_state == ADC_COMPLETE),
                "adcStopConversion(), #1",
                "invalid state");
    if (adcp->ad_state == ADC_RUNNING) {
        adc_lld_stop_conversion(adcp);
        adcp->ad_grpp  = NULL;
        adcp->ad_state = ADC_READY;
        chSemResetI(&adcp->ad_sem, 0);
        chSchRescheduleS();
    }
    else
        adcp->ad_state = ADC_READY;
    chSysUnlock();
}

static THD_FUNCTION(can_reader_thread, arg)
{
	t_hydra_console *con;
	con = arg;
	chRegSetThreadName("CAN reader");
	chThdSleepMilliseconds(10);
	can_rx_frame rx_msg;
	mode_config_proto_t* proto = &con->mode->proto;

	while (!chThdShouldTerminateX()) {
		if(bsp_can_rxne(proto->dev_num)) {
			chSysLock();
			bsp_can_read(proto->dev_num, &rx_msg);
			can_slcan_out(con, &rx_msg);
			chSysUnlock();
		} else {
			chThdYield();
		}
	}
	chThdExit((msg_t)1);
}