/**
 * @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;
}

/**
 * @brief   Input queue read with timeout.
 * @details This function reads a byte value from an input queue. If the queue
 *          is empty then the calling thread is suspended until a byte arrives
 *          in the queue or a timeout occurs.
 *
 * @param[in] iqp       pointer to an @p InputQueue structure
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              A byte value from the queue.
 * @retval Q_TIMEOUT    if the specified time expired.
 * @retval Q_RESET      if the queue was reset.
 *
 * @api
 */
msg_t chIQGetTimeout(InputQueue *iqp, systime_t time) {
  uint8_t b;
  msg_t msg;

  chSysLock();

  if (iqp->q_notify)
    iqp->q_notify(iqp);

  if ((msg = chSemWaitTimeoutS(&iqp->q_sem, time)) < RDY_OK) {
    chSysUnlock();
    return msg;
  }
  b = *iqp->q_rdptr++;
  if (iqp->q_rdptr >= iqp->q_top)
    iqp->q_rdptr = iqp->q_buffer;

  chSysUnlock();
  return b;
}

/**
 * @brief   Waits for all the specified events.
 * @details The function waits for all the events specified in @p events to
 *          become pending then the events are cleared and returned.
 *
 * @param[in] events    events that the function should wait
 *                      for, @p ALL_EVENTS requires all the events
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The mask of the served and cleared events.
 * @retval 0            if the operation has timed out.
 *
 * @api
 */
eventmask_t chEvtWaitAllTimeout(eventmask_t events, systime_t time) {
  thread_t *ctp = currp;

  chSysLock();
  if ((ctp->epending & events) != events) {
    if (TIME_IMMEDIATE == time) {
      chSysUnlock();
      return (eventmask_t)0;
    }
    ctp->u.ewmask = events;
    if (chSchGoSleepTimeoutS(CH_STATE_WTANDEVT, time) < MSG_OK) {
      chSysUnlock();
      return (eventmask_t)0;
    }
  }
  ctp->epending &= ~events;
  chSysUnlock();

  return events;
}

/**
 * @brief   Terminates the current thread.
 * @details The thread goes in the @p THD_STATE_FINAL state holding the
 *          specified exit status code, other threads can retrieve the
 *          exit status code by invoking the function @p chThdWait().
 * @post    Eventual code after this function will never be executed,
 *          this function never returns. The compiler has no way to
 *          know this so do not assume that the compiler would remove
 *          the dead code.
 *
 * @param[in] msg       thread exit code
 *
 * @api
 */
void chThdExit(msg_t msg) {
  Thread *tp = currp;

  chSysLock();
  tp->p_u.exitcode = msg;
#if defined(THREAD_EXT_EXIT_HOOK)
  THREAD_EXT_EXIT_HOOK(tp);
#endif
#if CH_USE_WAITEXIT
  while (notempty(&tp->p_waiting))
    chSchReadyI(list_remove(&tp->p_waiting));
#endif
#if CH_USE_REGISTRY
  /* Static threads are immediately removed from the registry because
     there is no memory to recover.*/
  if ((tp->p_flags & THD_MEM_MODE_MASK) == THD_MEM_MODE_STATIC)
    REG_REMOVE(tp);
#endif
  chSchGoSleepS(THD_STATE_FINAL);
}

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();
}

/**
 * @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();
}

u32 usart_support_write(void *sd, const u8 data[], u32 len)
{
  struct usart_tx_dma_state *s = &((struct usart_support_s *)sd)->tx;

  /* If there is no data to write, just return. */
  if (len == 0) return 0;

  chSysLock();

  /* Check if the write would cause a buffer overflow, if so only write up to
   * the end of the buffer. */
  u32 n_free = usart_tx_n_free(sd);
  if (len > n_free) {
    chSysUnlock();
    return 0;
  }

  u32 old_wr = s->wr;
  s->wr = (s->wr + len) % USART_TX_BUFFER_LEN;

  if (old_wr + len <= USART_TX_BUFFER_LEN)
    memcpy(&(s->buff[old_wr]), data, len);
  else {
    /* Deal with case where write wraps the buffer. */
    memcpy(&(s->buff[old_wr]), &data[0], USART_TX_BUFFER_LEN - old_wr);
    memcpy(&(s->buff[0]), &data[USART_TX_BUFFER_LEN - old_wr],
           len - (USART_TX_BUFFER_LEN - old_wr));
  }

  /* Check if there is a DMA transfer either in progress or waiting for its
   * interrupt to be serviced. Its very important to also check the interrupt
   * flag as EN will be cleared when the transfer finishes but we really need
   * to make sure the ISR has been run to finish up the bookkeeping for the
   * transfer. Also, make sure that this is done atomically without a DMA
   * interrupt squeezing in there. */
  if (!s->busy)
    dma_schedule(s);

  chSysUnlock();
  return len;
}

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;
}

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

  chThdCreateStatic(blinkWA, sizeof(blinkWA), NORMALPRIO, blink_thd, NULL);
  /* set alarm in near future */
  rtcGetTime(&RTCD1, &timespec);
  alarmspec.tv_sec = timespec.tv_sec + 30;
  rtcSetAlarm(&RTCD1, 0, &alarmspec);

  while (TRUE){
    chThdSleepSeconds(10);
    chSysLock();

    /* going to anabiosis*/
    PWR->CR |= (PWR_CR_PDDS | PWR_CR_LPDS | PWR_CR_CSBF | PWR_CR_CWUF);
    SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
    __WFI();
  }
  return 0;
}

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);
}

/**
 * @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;
}

/**
 * @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();
//  }
}

/**
 * @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;
}

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");
  }
}

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;
}

//-----------------------------------------------------------------------------
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   Brings the driver in a state safe for card removal.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 * @return              The operation status.
 *
 * @retval CH_SUCCESS   the operation succeeded and the driver is now
 *                      in the @p MMC_INSERTED state.
 * @retval CH_FAILED    the operation failed.
 *
 * @api
 */
bool_t mmcDisconnect(MMCDriver *mmcp) {

  chDbgCheck(mmcp != NULL, "mmcDisconnect");

  chSysLock();
  chDbgAssert((mmcp->state == BLK_ACTIVE) || (mmcp->state == BLK_READY),
              "mmcDisconnect(), #1", "invalid state");
  if (mmcp->state == BLK_ACTIVE) {
    chSysUnlock();
    return CH_SUCCESS;
  }
  mmcp->state = BLK_DISCONNECTING;
  chSysUnlock();

  /* Wait for the pending write operations to complete.*/
  sync(mmcp);

  spiStop(mmcp->config->spip);
  mmcp->state = BLK_ACTIVE;
  return CH_SUCCESS;
}

/**
 * @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();
}

/**
 * @brief   Waits for all the specified events.
 * @details The function waits for all the events specified in @p mask to
 *          become pending then the events are cleared and returned.
 *
 * @param[in] mask      mask of the event flags that the function should wait
 *                      for, @p ALL_EVENTS requires all the events
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The mask of the served and cleared events.
 * @retval 0            if the operation has timed out.
 *
 * @api
 */
eventmask_t chEvtWaitAllTimeout(eventmask_t mask, systime_t time) {
  Thread *ctp = currp;

  chSysLock();

  if ((ctp->p_epending & mask) != mask) {
    if (TIME_IMMEDIATE == time) {
      chSysUnlock();
      return (eventmask_t)0;
    }
    ctp->p_u.ewmask = mask;
    if (chSchGoSleepTimeoutS(THD_STATE_WTANDEVT, time) < RDY_OK) {
      chSysUnlock();
      return (eventmask_t)0;
    }
  }
  ctp->p_epending &= ~mask;

  chSysUnlock();
  return mask;
}

/**
 * @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);

  chSysLock();
#if CH_CFG_USE_MUTEXES == TRUE
  oldprio = currp->realprio;
  if ((currp->prio == currp->realprio) || (newprio > currp->prio)) {
    currp->prio = newprio;
  }
  currp->realprio = newprio;
#else
  oldprio = currp->prio;
  currp->prio = newprio;
#endif
  chSchRescheduleS();
  chSysUnlock();

  return oldprio;
}