void i2c_process(void) {
        //###Process incoming I²C-Data###

        //Target Colors
        if(twibuffer[0]<=170) target[0] = twibuffer[0];
        if(twibuffer[1]<=170) target[1] = twibuffer[1];
        if(twibuffer[2]<=170) target[2] = twibuffer[2];

        //Fade type
        fade[0] = twibuffer[3];
        fade[1] = twibuffer[4];
        fade[2] = twibuffer[5];

        //###Write new values to twibuffer so we can read current status###
        twibuffer[6] = color[0];
        twibuffer[7] = color[1];
        twibuffer[8] = color[2];

        twibuffer[9] = LOW_BYTE(colort[0]);
        twibuffer[10] = HIGH_BYTE(colort[0]);
        twibuffer[11] = LOW_BYTE(colort[1]);
        twibuffer[12] = HIGH_BYTE(colort[1]);
        twibuffer[13] = LOW_BYTE(colort[2]);
        twibuffer[14] = HIGH_BYTE(colort[2]);
}

static void  lcm_update_black(unsigned int x, unsigned int y,unsigned int width, unsigned int height, unsigned short data)
{
    unsigned int x0 = x;
    unsigned int y0 = y;
    unsigned int x1 = x0 + width;
    unsigned int y1 = y0 + height + 2;
    unsigned int k, i;

	set_lcm_register(0x2A00, HIGH_BYTE(x0),  0);
	set_lcm_register(0x2A01, LOW_BYTE(x0),  0);
	set_lcm_register(0x2A02, HIGH_BYTE(x1),  0);
	set_lcm_register(0x2A03, LOW_BYTE(x1),  0);
	set_lcm_register(0x2B00, HIGH_BYTE(y0),  0);
	set_lcm_register(0x2B01, LOW_BYTE(y0),  0);
	set_lcm_register(0x2B02, HIGH_BYTE(y1),  0);  
	set_lcm_register(0x2B03, LOW_BYTE(y1),  0);

	send_ctrl_cmd(0x2C00);

	for (i = x0; i < x1; i++)
	{
		for (k = y0; k < y1; k++)
		{
			send_data_cmd(data);
			send_data_cmd(data);
		}
	}
	
}

static void get_default_name (char *default_name, zword addr)
{

    if (addr != 0) {

	zbyte len;
	int i;

	LOW_BYTE (addr, len);
	addr++;

	for (i = 0; i < len; i++) {

	    zbyte c;

	    LOW_BYTE (addr, c);
	    addr++;

	    if (c >= 'A' && c <= 'Z')
		c += 'a' - 'A';

	    default_name[i] = c;

	}

	default_name[i] = 0;

	if (strchr (default_name, '.') == NULL)
	    strcpy (default_name + i, ".AUX");

    } else strcpy (default_name, auxilary_name);

}/* get_default_name */

/*
 * z_copy_table, copy a table or fill it with zeroes.
 *
 *	zargs[0] = address of table
 * 	zargs[1] = destination address or 0 for fill
 *	zargs[2] = size of table
 *
 * Note: Copying is safe even when source and destination overlap; but
 *       if zargs[1] is negative the table _must_ be copied forwards.
 *
 */
void z_copy_table (void)
{
    zword addr;
    zword size = zargs[2];
    zbyte value;
    int i;

    if (zargs[1] == 0)      				/* zero table */

	for (i = 0; i < size; i++)
	    storeb ((zword) (zargs[0] + i), 0);

    else if ((short) size < 0 || zargs[0] > zargs[1])	/* copy forwards */

	for (i = 0; i < (((short) size < 0) ? - (short) size : size); i++) {
	    addr = zargs[0] + i;
	    LOW_BYTE (addr, value)
	    storeb ((zword) (zargs[1] + i), value);
	}

    else						/* copy backwards */

	for (i = size - 1; i >= 0; i--) {
	    addr = zargs[0] + i;
	    LOW_BYTE (addr, value)
	    storeb ((zword) (zargs[1] + i), value);
	}

}/* z_copy_table */

static void  lcm_update_black(unsigned int x, unsigned int y,unsigned int width, unsigned int height, unsigned short data)
{
    unsigned int x0 = x;
    unsigned int y0 = y;
    unsigned int x1 = x0 + width;
    unsigned int y1 = y0 + height + 2;
    unsigned int k, i;
	
	send_ctrl_cmd(0x2A);
	send_data_cmd(HIGH_BYTE(x0));
	send_data_cmd(LOW_BYTE(x0));
	send_data_cmd(HIGH_BYTE(x1));
	send_data_cmd(LOW_BYTE(x1));

	send_ctrl_cmd(0x2B);
	send_data_cmd(HIGH_BYTE(y0));
	send_data_cmd(LOW_BYTE(y0));
	send_data_cmd(HIGH_BYTE(y1));
	send_data_cmd(LOW_BYTE(y1));
	send_ctrl_cmd(0x2C);
	
	for (i = x0; i < x1; i++)
	{
		for (k = y0; k < y1; k++)
		{
			send_data_cmd(data);
		}
	}
}

// script to rotate in place through deg degrees
// deg > 0 turn CCW; deg < 0 turn CW
void create_script_turn(int deg, int speed) { // degrees, vel in mm/sec
	CREATE_BUSY;
	create_write_byte(152); // start script
	create_write_byte(17);  // script length
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 0
	create_write_byte(137); // move 
	create_write_byte(HIGH_BYTE(speed));
	create_write_byte(LOW_BYTE(speed));
	if (deg > 0){ // CCW case 
		create_write_byte(0); 
	create_write_byte(1); }
	else { // CW case
		create_write_byte(255);
	create_write_byte(255); }
	create_write_byte(157); // wait for angle done
	create_write_byte(HIGH_BYTE(deg));
	create_write_byte(LOW_BYTE(deg));
	create_write_byte(137); // stop move 
	create_write_byte(0);   // no speed
	create_write_byte(0);
	create_write_byte(0);   // no angle
	create_write_byte(0);   
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 1
	// end of script
	create_write_byte(153); // run script
	CREATE_FREE;
}

// script to trace an arc of radius rad until deg is reach
// NOTE: if the turn is not in the direction of deg, the arc won't end
void create_script_arc(int rad, int deg, int speed) { // rad in mm, degrees, vel in mm/sec
	CREATE_BUSY;
	create_write_byte(152); // start script
	create_write_byte(17);  // script length
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 0
	create_write_byte(137); // move 
	create_write_byte(HIGH_BYTE(speed));
	create_write_byte(LOW_BYTE(speed));
	create_write_byte(HIGH_BYTE(rad)); 
	create_write_byte(HIGH_BYTE(rad)); 
	create_write_byte(157); // wait for angle done
	create_write_byte(HIGH_BYTE(deg));
	create_write_byte(LOW_BYTE(deg));
	create_write_byte(137); // stop move 
	create_write_byte(0);   // no speed
	create_write_byte(0);
	create_write_byte(0);   // no angle
	create_write_byte(0);   
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 1
	// end of script
	create_write_byte(153); // run script
	CREATE_FREE;
}

// script to move with individual motor control
// dist mm with r_speed mm/sec on right wheel and l_speed on left
void create_script_move_direct(int dist, int r_speed, int l_speed) { 
	CREATE_BUSY;
	create_write_byte(152); // start script
	create_write_byte(17);  // script length
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 0
	create_write_byte(145); // move 
	create_write_byte(HIGH_BYTE(r_speed));
	create_write_byte(LOW_BYTE(r_speed));
	create_write_byte(HIGH_BYTE(l_speed));
	create_write_byte(LOW_BYTE(l_speed));
	create_write_byte(156); // wait for distance done
	create_write_byte(HIGH_BYTE(dist));
	create_write_byte(LOW_BYTE(dist));
	create_write_byte(137); // stop move 
	create_write_byte(0);   // no speed
	create_write_byte(0);
	create_write_byte(0);   // no angle
	create_write_byte(0);   
	create_write_byte(147); // on pin 20
	create_write_byte(0);   //   output 1
	// end of script
	create_write_byte(153); // run script
	CREATE_FREE;
}

void _dma_io(u8_t DMA_channel, unsigned char page, unsigned int offset, unsigned int length, u8_t mode)
{
    /* Don't let anyone else mess up what we're doing. */
    asm_disable_interrupt();

    /* Set up the DMA channel so we can use it.  This tells the DMA */
    /* that we're going to be using this channel.  (It's masked) */
    io_outb(MaskReg[DMA_channel], 0x04 | DMA_channel);

    /* Clear any data transfers that are currently executing. */
    io_outb(ClearReg[DMA_channel], 0x00);

    /* Send the specified mode to the DMA. */
    io_outb(ModeReg[DMA_channel], mode);

    /* Send the offset address.  The first byte is the low base offset, the */
    /* second byte is the high offset. */
    io_outb(AddrPort[DMA_channel], LOW_BYTE(offset));
    io_outb(AddrPort[DMA_channel], HI_BYTE(offset));

    /* Send the physical page that the data lies on. */
    io_outb(PagePort[DMA_channel], page);

    /* Send the length of the data.  Again, low byte first. */
    io_outb(CountPort[DMA_channel], LOW_BYTE(length));
    io_outb(CountPort[DMA_channel], HI_BYTE(length));

    /* Ok, we're done.  Enable the DMA channel (clear the mask). */
    io_outb(MaskReg[DMA_channel], DMA_channel);

    /* Re-enable interrupts before we leave. */
    asm_enable_interrupt();
}

// Attributes stored in RAM are always re-initialized upon start-up.
void emAfRf4ceMsoInitAttributes(void)
{
  uint8_t i, j;

  MEMSET(emAfRf4ceMsoRibAttributes,
         0x00,
         sizeof(EmAfRf4ceMsoRibAttributes)*EMBER_RF4CE_PAIRING_TABLE_SIZE);

  // Initialize the device type of all the peripheral ID entries to 0xFF, which
  // means "unused" in this context.
  for(i=0; i<EMBER_RF4CE_PAIRING_TABLE_SIZE; i++) {
    for(j=0; j<EMBER_AF_PLUGIN_RF4CE_MSO_PERIPHERAL_ID_ENTRIES; j++) {
      emAfRf4ceMsoRibAttributes[i].peripheralIds[j].deviceType = 0xFF;
    }

    // Initialize the validation configuration attribute fields to the default
    // values.
    emAfRf4ceMsoRibAttributes[i].validationConfiguration[0] =
        LOW_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_LINK_LOST_WAIT_TIME_MS);
    emAfRf4ceMsoRibAttributes[i].validationConfiguration[1] =
        HIGH_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_LINK_LOST_WAIT_TIME_MS);
    emAfRf4ceMsoRibAttributes[i].validationConfiguration[2] =
        LOW_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_AUTO_CHECK_VALIDATION_PERIOD_MS);
    emAfRf4ceMsoRibAttributes[i].validationConfiguration[3] =
        HIGH_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_AUTO_CHECK_VALIDATION_PERIOD_MS);
  }
}

static void sw_clear_panel(unsigned int color)
{
    unsigned int x0 = 0;
    unsigned int y0 = 0;
    unsigned int x1 = x0 + FRAME_WIDTH - 1;
    unsigned int y1 = y0 + FRAME_HEIGHT - 1;

    unsigned int x, y;

    send_ctrl_cmd(0x2A); 
    send_data_cmd(HIGH_BYTE(x0));
  	send_data_cmd(LOW_BYTE(x0));
    send_data_cmd(HIGH_BYTE(x1));
  	send_data_cmd(LOW_BYTE(x1));

  	send_ctrl_cmd(0x2B); 
    send_data_cmd(HIGH_BYTE(y0));
  	send_data_cmd(LOW_BYTE(y0));
    send_data_cmd(HIGH_BYTE(y1));
  	send_data_cmd(LOW_BYTE(y1));

	send_ctrl_cmd(0x2C);    // send DDRAM set

    // 18-bit mode (256K color) coding
    for (y = y0; y <= y1; ++ y) {
        for (x = x0; x <= x1; ++ x) {
            lcm_util.send_data(color);
        }
    }
}

void Roomba_Drive(int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

void Roomba_Direct_Drive(int16_t right_velocity, int16_t left_velocity)
{
	uart_putchar(DIRECT_DRIVE);
	uart_putchar(HIGH_BYTE(right_velocity));
	uart_putchar(LOW_BYTE(right_velocity));
	uart_putchar(HIGH_BYTE(left_velocity));
	uart_putchar(LOW_BYTE(left_velocity));
}

// this function drives the right wheel at r_speed and the left wheel at l_speed
// speeds for all of these functions are +/-500mm/sec.
void create_drive_direct(int r_speed, int l_speed)
{
	CREATE_BUSY;
	create_write_byte(145);
	create_write_byte(HIGH_BYTE(r_speed));
	create_write_byte(LOW_BYTE(r_speed));
	create_write_byte(HIGH_BYTE(l_speed));
	create_write_byte(LOW_BYTE(l_speed));
	CREATE_FREE;
}    

// This command drives the robot along a curve with radius (in mm) radius; and at a speed (mm/sec) of speed
// a radius of 32767 will drive the robot straight
// a radius of 1 will spin the robot CCW
// a radius of -1 will spin the robot CW
// Negative radii will be right turns, positive radii left turns
void create_drive (int speed, int radius)
{
	CREATE_BUSY;
	create_write_byte(137);
	create_write_byte(HIGH_BYTE(speed));
	create_write_byte(LOW_BYTE(speed));
	create_write_byte(HIGH_BYTE(radius));
	create_write_byte(LOW_BYTE(radius));
	CREATE_FREE;
}

// special version of command spins robot CCW with the wheels turning at speed speed
void create_spin_CCW (int speed)
{
	CREATE_BUSY;
	create_write_byte(137);
	create_write_byte(HIGH_BYTE(speed));
	create_write_byte(LOW_BYTE(speed));
	create_write_byte(HIGH_BYTE(1));
	create_write_byte(LOW_BYTE(1));
	CREATE_FREE;
}

// special version of command above drives robot at speed speed.  Negative speed will drive robot backwards
void create_drive_straight (int speed)
{
	CREATE_BUSY;
	create_write_byte(137);
	create_write_byte(HIGH_BYTE(speed));
	create_write_byte(LOW_BYTE(speed));
	create_write_byte(HIGH_BYTE(32767));
	create_write_byte(LOW_BYTE(32767));
	CREATE_FREE;
}

static void
get_string_descriptor()
{
#if OLD_STRING_DESCR
  if(LOW_BYTE(usb_setup_buffer.wValue) == 0) {
    usb_send_ctrl_response((const unsigned char *)string_languages->
                           lang_descr, string_languages->lang_descr->bLength);
  } else {
    const struct usb_st_string_descriptor *descriptor;

    unsigned char l;

    const struct usb_st_string_descriptor *const *table;

    const struct usb_st_string_language_map *map;

    if(LOW_BYTE(usb_setup_buffer.wValue) > string_languages->max_index) {
      usb_error_stall();
      return;
    }
    l = string_languages->num_lang;
    map = string_languages->map;
    table = map->descriptors;   /* Use first table if language not found */
    while(l > 0) {
      if(map->lang_id == usb_setup_buffer.wIndex) {
        table = map->descriptors;
        break;
      }
      map++;
      l--;
    }
    PRINTF("Lang id %04x = table %p\n", usb_setup_buffer.wIndex,
           (void *)table);
    descriptor = table[LOW_BYTE(usb_setup_buffer.wValue) - 1];
    usb_send_ctrl_response((const unsigned char *)descriptor,
                           descriptor->bLength);
  }
#else
  const struct usb_st_string_descriptor *descriptor;

  descriptor = (struct usb_st_string_descriptor *)
    usb_class_get_string_descriptor(usb_setup_buffer.wIndex,
                                    LOW_BYTE(usb_setup_buffer.wValue));
  if(!descriptor) {
    usb_error_stall();
    return;
  }
  usb_send_ctrl_response((const unsigned char *)descriptor,
                         descriptor->bLength);
#endif
}

EmberStatus ezspEndDeviceBindRequest(EmberNodeId localNodeId,
                                     EmberEUI64 localEui64,
                                     uint8_t endpoint,
                                     uint16_t profile,
                                     uint8_t inCount,
                                     uint8_t outCount,
                                     uint16_t *inClusters,
                                     uint16_t *outClusters,
                                     EmberApsOption options)
{
  uint8_t i;
  uint8_t *payload = zigDevRequestBuffer + ZDO_MESSAGE_OVERHEAD;
  uint8_t offset = ZDO_MESSAGE_OVERHEAD + 14;  // Add 2 bytes for our NWK Address 
                                             // Add 8 bytes for our EUI64
                                             // Add 1 byte for endpoint
                                             // Add 2 bytes for Profile Id
                                             // Add 1 byte for in Cluster Count
  uint8_t length = (offset + 
                  (inCount * 2) +  // Times 2 for 2 byte Clusters
                  1 +              // Out Cluster Count
                  (outCount * 2)); // Times 2 for 2 byte Clusters
  
  if (length > EZSP_MAX_FRAME_LENGTH)
    return EMBER_NO_BUFFERS;

  payload[0] = LOW_BYTE(localNodeId);
  payload[1] = HIGH_BYTE(localNodeId);
  MEMMOVE(payload + 2, localEui64, 8);
  payload[10] = endpoint;
  payload[11] = LOW_BYTE(profile);
  payload[12] = HIGH_BYTE(profile);
  payload[13] = inCount;

  for (i = 0; i < inCount; i++) {
    zigDevRequestBuffer[(i * 2) + offset] = LOW_BYTE(inClusters[i]);
    zigDevRequestBuffer[(i * 2) + offset + 1] = HIGH_BYTE(inClusters[i]);
  }
  offset += (inCount * 2);
  zigDevRequestBuffer[offset] = outCount;
  offset++;
  for (i = 0; i < outCount; i++) {
    zigDevRequestBuffer[(i * 2) + offset] = LOW_BYTE(outClusters[i]);
    zigDevRequestBuffer[(i * 2) + offset + 1] = HIGH_BYTE(outClusters[i]);
  }

  return sendZigDevRequestBuffer(EMBER_ZIGBEE_COORDINATOR_ADDRESS,
                                 END_DEVICE_BIND_REQUEST,
                                 options,
                                 length);
}

/**@brief Function for simulating a bike speed event. 
 * 
 * @param[in] p_context Timeout context.
 */
static void speed_pulse_event_simulate(void * p_context)
{  
    // Current speed event time is the previous event time added with the event interval. 
    m_speed_event_time += SPEED_EVENT_INTERVAL;
    
    // Update current speed event time.   
    m_tx_buffer[SPEED_EVENT_TIME_LSB_INDEX] = LOW_BYTE(m_speed_event_time);    
    m_tx_buffer[SPEED_EVENT_TIME_MSB_INDEX] = HIGH_BYTE(m_speed_event_time);            

    // Update current speed event count.    
    ++m_speed_revolution_count;
    m_tx_buffer[CUMULATIVE_SPEED_LSB_INDEX] = LOW_BYTE(m_speed_revolution_count);    
    m_tx_buffer[CUMULATIVE_SPEED_MSB_INDEX] = HIGH_BYTE(m_speed_revolution_count);                   
}