err_t sys_mbox_trypost(sys_mbox_t *mbox, void *msg) {

  if (chMBPost(*mbox, (msg_t)msg, TIME_IMMEDIATE) == MSG_TIMEOUT) {
    SYS_STATS_INC(mbox.err);
    return ERR_MEM;
  }
  return ERR_OK;
}

// ================================ Inner use ==================================
void Sound_t::AddCmd(uint8_t AAddr, uint16_t AData) {
    VsCmd_t FCmd;
    FCmd.OpCode = VS_WRITE_OPCODE;
    FCmd.Address = AAddr;
    FCmd.Data = __REV16(AData);
    // Add cmd to queue
    chMBPost(&CmdBox, FCmd.Msg, TIME_INFINITE);
    StartTransmissionIfNotBusy();
}

	void gdispDrawLine(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWLINE);
		p->drawline.x0 = x0;
		p->drawline.y0 = y0;
		p->drawline.x1 = x1;
		p->drawline.y1 = y1;
		p->drawline.color = color;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

	void gdispFillArea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLAREA);
		p->fillarea.x = x;
		p->fillarea.y = y;
		p->fillarea.cx = cx;
		p->fillarea.cy = cy;
		p->fillarea.color = color;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

	void gdispFillEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLELLIPSE);
		p->fillellipse.x = x;
		p->fillellipse.y = y;
		p->fillellipse.a = a;
		p->fillellipse.b = b;
		p->fillellipse.color = color;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

	void gdispDrawChar(coord_t x, coord_t y, char c, font_t font, color_t color) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWCHAR);
		p->drawchar.x = x;
		p->drawchar.y = y;
		p->drawchar.c = c;
		p->drawchar.font = font;
		p->drawchar.color = color;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

/*
 * Test worker threads.
 */
static THD_FUNCTION(irq_storm_thread, arg) {
  static volatile unsigned x = 0;
  static unsigned cnt = 0;
  unsigned me = (unsigned)arg;
  unsigned target;
  unsigned r;
  msg_t msg;

  chRegSetThreadName("irq_storm");

  /* Thread loop, until terminated.*/
  while (chThdShouldTerminateX() == false) {

    /* Waiting for a message.*/
   chMBFetch(&mb[me], &msg, TIME_INFINITE);

#if IRQ_STORM_CFG_RANDOMIZE != FALSE
   /* Pseudo-random delay.*/
   {
     chSysLock();
     r = rand() & 15;
     chSysUnlock();
     while (r--)
       x++;
   }
#else /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
   /* Fixed delay.*/
   {
     r = me >> 4;
     while (r--)
       x++;
   }
#endif /* IRQ_STORM_CFG_RANDOMIZE == FALSE */

    /* Deciding in which direction to re-send the message.*/
    if (msg == MSG_SEND_LEFT)
      target = me - 1;
    else
      target = me + 1;

    if (target < IRQ_STORM_CFG_NUM_THREADS) {
      /* If this thread is not at the end of a chain re-sending the message,
         note this check works because the variable target is unsigned.*/
      msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
      if (msg != MSG_OK)
        saturated = TRUE;
    }
    else {
      /* Provides a visual feedback about the system.*/
      if (++cnt >= 500) {
        cnt = 0;
        palTogglePad(config->port, config->pad);
      }
    }
  }
}

/*
 * log_event
 *
 * This is how you send things to the logger so it can log them.
 * The id parameter must point to an array of at least SDC_MSG_ID_BYTES chars.
 * The data_length parameter must accurately represent the length of the array
 * pointed to by the data parameter, and this length must be no longer than
 * SDC_MSG_MAX_PAYLOAD_BYTES. The returned boolean indicates whether the event
 * was succesfully posted (true) or failed because the buffer of events to be
 * logged was full (false).
 */
bool log_event(const char* id, const uint8_t* data, uint16_t data_length) {
	msg_t status;

	GENERIC_message* msg = make_msg(id, data, data_length);
	if (msg == NULL) return false;

	status = chMBPost(&event_mail, (msg_t) msg, TIME_IMMEDIATE);

	return status == RDY_OK;
}

	void gdispFillArc(coord_t x, coord_t y, coord_t radius, coord_t start, coord_t end, color_t color) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLARC);
		p->fillarc.x = x;
		p->fillarc.y = y;
		p->fillarc.radius = radius;
		p->fillarc.start = start;
		p->fillarc.end = end;
		p->fillarc.color = color;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

	void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLCHAR);
		p->fillchar.x = x;
		p->fillchar.y = y;
		p->fillchar.c = c;
		p->fillchar.font = font;
		p->fillchar.color = color;
		p->fillchar.bgcolor = bgcolor;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

	void gdispVerticalScroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_VERTICALSCROLL);
		p->verticalscroll.x = x;
		p->verticalscroll.y = y;
		p->verticalscroll.cx = cx;
		p->verticalscroll.cy = cy;
		p->verticalscroll.lines = lines;
		p->verticalscroll.bgcolor = bgcolor;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

void rfm69_log_s64(uint8_t channel, int64_t data)
{
    char *msg;
    msg = (void*)chPoolAlloc(&rfm69_mp);
    msg[4] = (char)(1 | conf.location << 4);
    msg[5] = (char)channel;
    memcpy(msg, (void*)&halGetCounterValue(), 4);
    memcpy(&msg[8], &data, 8);
    chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}

void rfm69_log_c(uint8_t channel, const char* data)
{
    volatile char *msg;
    msg = (char*)chPoolAlloc(&rfm69_mp);
    msg[4] = (char)(0 | conf.location << 4);
    msg[5] = (char)channel;
    memcpy((void*)msg, (void*)&halGetCounterValue(), 4);
    memcpy((void*)&msg[8], data, 8);
    chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}

static msg_t WorkerThread(void *arg) {
  static volatile unsigned x = 0;
  static unsigned cnt = 0;
  unsigned me = (unsigned)arg;
  unsigned target;
  unsigned r;
  msg_t msg;

  chRegSetThreadName("worker");

  /* Work loop.*/
  while (TRUE) {
    /* Waiting for a message.*/
   chMBFetch(&mb[me], &msg, TIME_INFINITE);

#if RANDOMIZE
   /* Pseudo-random delay.*/
   {
     chSysLock();
     r = rand() & 15;
     chSysUnlock();
     while (r--)
       x++;
   }
#else
   /* Fixed delay.*/
   {
     r = me >> 4;
     while (r--)
       x++;
   }
#endif

    /* Deciding in which direction to re-send the message.*/
    if (msg == MSG_SEND_LEFT)
      target = me - 1;
    else
      target = me + 1;

    if (target < NUM_THREADS) {
      /* If this thread is not at the end of a chain re-sending the message,
         note this check works because the variable target is unsigned.*/
      msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
      if (msg != RDY_OK)
        saturated = TRUE;
    }
    else {
      /* Provides a visual feedback about the system.*/
      if (++cnt >= 500) {
        cnt = 0;
        palTogglePad(GPIO0, GPIO0_LED2);
      }
    }
  }
}

void rfm69_log_f(uint8_t channel, float data_a, float data_b)
{
    char *msg;
    msg = (void*)chPoolAlloc(&rfm69_mp);
    msg[4] = (char)(9 | conf.location << 4);
    msg[5] = (char)channel;
    memcpy(msg, (void*)&halGetCounterValue(), 4);
    memcpy(&msg[8],  &data_a, 4);
    memcpy(&msg[12], &data_b, 4);
    chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}

// add a next setpoint for the stepper to go to, will wait if queue is full
void setpointAdd (const Setpoint& s) {
    // TODO: silly approach, should use first/last indices into circular buffer
    for (int i = 0; i <= SETPOINT_QUEUE_SIZE; ++i)
        if (!isInUse(i)) {
            setInUse(i);
            setpoint.setpoints[i] = s;
            chMBPost(&setpoint.mailbox, i, TIME_INFINITE);
            return;
        }
    // never reached
}

	void gdispBlitAreaEx(coord_t x, coord_t y, coord_t cx, coord_t cy, coord_t srcx, coord_t srcy, coord_t srccx, const pixel_t *buffer) {
		gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_BLITAREA);
		p->blitarea.x = x;
		p->blitarea.y = y;
		p->blitarea.cx = cx;
		p->blitarea.cy = cy;
		p->blitarea.srcx = srcx;
		p->blitarea.srcy = srcy;
		p->blitarea.srccx = srccx;
		p->blitarea.buffer = buffer;
		chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
	}

static msg_t tUsbRx(void *arg) {
  (void)arg;
  chRegSetThreadName("usbRx");

  enum {UsbRxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};

  usbPacket *usbBufp;

  EventListener elUsbRxComplete;
  EventListener elUsbReset;
  EventListener elUsbConfigured;

  eventmask_t   activeEvents;


  chEvtRegister(&esUsbRxComplete, &elUsbRxComplete, UsbRxComleteID);
  chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
  chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);

  // Wait for the USB system to be configured.
  chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
  chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));

  while (TRUE) {

    // Allocate buffer space for reception of package in the sysctrl mempool
    usbBufp = usbAllocMailboxBuffer();

    // Prepare receive operation and initiate the usb system to listen
    usbPrepareReceive(usbp, EP_OUT, usbBufp->packet, 64);
    chSysLock();
    usbStartReceiveI(usbp, EP_OUT);
    chSysUnlock();

    // Wait for events from the USB system
    activeEvents = chEvtWaitAny(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));

    if (activeEvents == EVENT_MASK(UsbResetID)) {
      // If the system was reset, clean up and wait for new configure.
      usbFreeMailboxBuffer (usbBufp);
      chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
      chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
    }
    else {
      // Post pagckage to sysctrl if receive was successful
      usbBufp->size = ep2outstate.rxcnt;
      chMBPost (&usbRXMailbox, (msg_t)usbBufp, TIME_INFINITE);
    }
  }

  return 0;
}

void rfm69_log_u16(uint8_t channel, uint16_t data_a, uint16_t data_b,
                                      uint16_t data_c, uint16_t data_d)
{
    char *msg;
    msg = (void*)chPoolAlloc(&rfm69_mp);
    msg[4] = (char)(6 | conf.location << 4);
    msg[5] = (char)channel;
    memcpy(msg, (void*)&halGetCounterValue(), 4);
    memcpy(&msg[8],  &data_a, 2);
    memcpy(&msg[10], &data_b, 2);
    memcpy(&msg[12], &data_c, 2);
    memcpy(&msg[14], &data_d, 2);
    chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}

// Loops constantly in the background.
void matrix_scan_user(void) {
  uint8_t page;
  uint8_t led_pin_byte;
  msg_t msg;

  if (backlight_status_global == 0) {//backlight is off, skip the rest
    return;
  }

  if (led_layer_state != layer_state && led_mode_global != GAME && led_mode_global != ALL) {
    //check mode
    //Turn on layer indicator or page depending on mode
    switch(led_mode_global) {
      case MODE_FLASH: //flash preset page leds then single indicator
        page = biton32(layer_state) > max_pages ? 7 : biton32(layer_state);
        msg=(page << 8) | DISPLAY_PAGE;
        chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
        chThdSleepMilliseconds(500);
        //flow to display single layer leds

      case MODE_SINGLE: //light layer indicators for all active layers
        led_pin_byte = layer_state & 0xFF;
        msg=(7 << 8) | DISPLAY_PAGE;
        chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
        msg=(1 << 16) | (led_pin_byte << 8) | SET_FULL_ROW;
        chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
        break;

      case MODE_PAGE: //display pre-defined led page
        page = biton32(layer_state) > max_pages ? 7 : biton32(layer_state);
        msg=(page << 8) | DISPLAY_PAGE;
        chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
        break;
      }
  led_layer_state = layer_state;
  }
}