#include <stdlib.h>

#include "pico/stdlib.h"
#include "tusb.h"
#include "hardware/adc.h"

#include "corsair_strafe2.h"
#include "usb_descriptors.h"

static bool sending = false;
static absolute_time_t lastTime;
static bool backlight = false;

static unsigned char buf[BUF_SIZE];
static uint8_t buf_idx=0;
static uint8_t packets_sent=0;
static uint8_t color_idx = 0;
static uint8_t channel = 1;
static uint8_t channel_packet = 0;
static uint8_t curcolor = 0;

static uint8_t input;
static uint8_t bit=0;
static uint8_t byte=0;
static uint8_t boot_pos=0;

// used to map Corsair's keycode to standard keyboard scan code
static const int16_t nkro_key[NKRO_BUF_SIZE*8] = {
49, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 61, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 53, 51, 28, 34, 16, 29, 31, 36, 32, 20, 26, 27, 55, 65, 12, 30, 15, 17, 18, 19, 21, 22, 23, 59, 60, 1, 108, 37, 35, 14, 33, 13, 25, 24, 62, 63, 64, 0, 3, 2, 153, 52, 152, 144, 6, 7, 109, -1, 258, 77, 78, 79, 80, 81, 82, 83, 56, 57, 58, 48, 143, 54, 145, 50, 84, 85, 86, 5, 4, 90, 88, 89, 87, 253, 135, 128, 242, -1, 243, 91, 92, 93, 94, 95, 96, 103, 104, 105, 212, 100, 101, 102, 97, 98, 99, 106, 107, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 136, 137, 255, 249, 250, 251, 226, 227, 228, 229, 230, 231, 240, 241, 147, 146, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1
};

void get_light() {
	// get measurement from ADC and set a flag for the backlight based on
	// reading from the LDR
	if (adc_read() > 400) {
		backlight = false;
	} else {
		backlight = true;
	}
}

void rgb_task(uint8_t dev_addr, uint8_t instance, uint8_t report_id) {
	// RGB color data is sent in multiple packets - determine whether we
	// are in the middle of sending color data, and if so, send the next
	// packet; otherwise begin sending the next round of color data 
	// 1 second after the last packet of the last round
	if (sending) {
		if ( absolute_time_diff_us(lastTime, get_absolute_time()) > 100000) {
			// set color to different intensities of white based on whether
			// backlight is flagged as "on" or "off"
			if( backlight) {
				// turn on backlight at a bright level (#505050)
				send_color(dev_addr, instance, report_id, 0x50, 0x50, 0x50);
			} else{
				// turn on backlight at a dim level (#101010)
				send_color(dev_addr, instance, report_id, 0x10, 0x10, 0x10);
			}
			lastTime = get_absolute_time();
		}
	} else {
		if ( absolute_time_diff_us(lastTime, get_absolute_time()) > 1000000) {
			send_initial(dev_addr, instance, report_id);
			lastTime = get_absolute_time();
		}
	}
}

// send a single color packet
void send_color(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t red, uint8_t green, uint8_t blue) {
	memset(buf, 0x00, BUF_SIZE);

	buf_idx = 0;

	// packets 1 and packets 2 are commands to activate software control
	// mode and initiate sending of key-by-key lighting data
	if(packets_sent==1){
		buf[0x00] = 0x07;
		buf[0x01] = 0x05;
		buf[0x02] = 0x02;
		buf[0x04] = 0x03;
	}
	if(packets_sent==2){
		buf[0x00] = 0x07;
		buf[0x01] = 0x05;
		buf[0x02] = 0x08;
		buf[0x04] = 0x01;
	}
	// send RGB data - each color channel submits all keys for the channel
	// before moving to the next channel, i.e. all R values for all keys,
	// all G values for all keys, then all B values for all keys
	if(packets_sent>2){
		if(color_idx < NUM_KEYS) {
			channel_packet++;
			buf[0x00] = 0x7F;
			buf[0x01] = channel_packet;
			if(NUM_KEYS - color_idx > 60) {
				buf[0x02] = 60;
			} else{
				buf[0x02] = NUM_KEYS-color_idx;
			}

			// determine value to send based on which channel we are currently
			// transmitting
			if(channel == 1){
				curcolor = red;
			}
			if(channel == 2){
				curcolor = green;
			}
			if(channel == 3){
				curcolor = blue;
			}

			buf_idx=4;
			// set all keys in this packet to the value for the R/G/B channel
			while(color_idx < NUM_KEYS && buf_idx < BUF_SIZE){
				buf[buf_idx] = curcolor;
				buf_idx++;
				color_idx++;
			}
		} else{
			// start a new packet stating we are done sending 24-bit
			// color packet for the currently selected channel and how many
			// packets were sent for that color channel
			buf[0x00] = 0x07;
			buf[0x01] = 0x28;
			buf[0x02] = channel;
			buf[0x03] = channel_packet;
			channel++;
			channel_packet=0;
			color_idx = 0;
			// final byte says whether to expect another channel (0x01) or
			// whether we have sent the final channel (0x02)
			if (channel>3) {
				buf[0x04] = 0x02;
				sending = false;
			} else {
				buf[0x04] = 0x01;
			}
		}
	}
 	if(tuh_hid_send_report(dev_addr, instance, report_id, buf, BUF_SIZE))
	{
		// packet sent sucessfully
	}


	packets_sent++;
}


void send_initial(uint8_t dev_addr, uint8_t instance, uint8_t report_id) {
	memset(buf, 0x00, BUF_SIZE);

	color_idx = 0;

	// setup and send initialization packet
	buf[0x00] = 0x07;
	buf[0x01] = 0x04;
	buf[0x02] = 0x02;

	if (tuh_hid_send_report(dev_addr, instance, report_id, buf, BUF_SIZE))
	{
		sending = true;
		packets_sent=1;
		channel = 1;
		channel_packet = 0;
	}
}

// initiate the ADC and setup reading from ADC2
void startADC() {
	stdio_init_all();
	adc_init();
	adc_gpio_init(28);
	adc_select_input(2);
}

/*
// take bitmap NKRO keypress data and turn it into boot protocol HID report
void nkro2boot(uint8_t const* nkro_report, uint8_t* boot_report, uint16_t len) {
	memset(boot_report, 0x00, 8);
	(void) len;
	// copy modifiers
	boot_report[0] = nkro_report[0];
	boot_pos=2;

	// set regular keyboard keys
	for (byte=1; byte<NKRO_BUF_SIZE; byte++) {
		input = nkro_report[byte];
		for (bit=0; bit < 8; bit++) {
			if ( (input >> bit) & 1) {
				boot_report[boot_pos] = byte*8+bit-8;
				boot_pos++;
				if(boot_pos>=8) {
					return;
				}
			}
		}
	}
}
*/

// take Corsair's single key up/down update message and update the
// NKRO HID report bitmap
uint8_t corsair2nkro(uint8_t const* corsair_report, uint8_t* nkro_report, uint16_t len) {
	memset(nkro_report, 0x00, NKRO_BUF_SIZE);

	(void) len;

	// search for media keys
	input = corsair_report[12];
	if( (input >> 1) & 1) {
		// mute
		nkro_report[2] = 0xE2;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	if( (input >> 2) & 1) {
		// stop
		nkro_report[2] = 0xB7;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	if( (input >> 3) & 1) {
		// back
		nkro_report[2] = 0xB6;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	if( (input >> 4) & 1) {
		// pause/play
		nkro_report[2] = 0xCD;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	if( (input >> 5) & 1) {
		// mute
		nkro_report[2] = 0xB5;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	input = corsair_report[16];
	if( (input >> 2) & 1) {
		// volume up
		nkro_report[2] = 0xE9;
		return REPORT_ID_CONSUMER_CONTROL;
	}
	if( (input >> 3) & 1) {
		// volume down
		nkro_report[2] = 0xEA;
		return REPORT_ID_CONSUMER_CONTROL;
	}

	// set regular keyboard keys
	for (byte=0; byte<NKRO_BUF_SIZE; byte++) {
		input = corsair_report[byte];
		for (bit=0; bit < 8; bit++) {
			if ( (input >> bit) & 1) {
				if (nkro_key[byte*8+bit] >= 0) {
					SET_KEYBIT(nkro_report, nkro_key[byte*8+bit]);
				}
			}
		}
	}
	return REPORT_ID_KEYBOARD;
}