#ifndef IO_H
#define IO_H

#define LEFT_B ((remoteState->dpadL) ? DPAD_LEFT:0) | ((remoteState->dpadL) ? DPAD_RIGHT:0) | ((remoteState->dpadD) ? DPAD_DOWN : 0) | ((remoteState->dpadU) ? DPAD_UP : 0) | ((remoteState->plus) ? PLUS : 0) | (OTHER0 & 0) | (OTHER1 & 0) | (UNKNOWN & 0)
#define RIGHT_B ((remoteState->two) ? TWO : 0) | ((remoteState->one) ? ONE : 0) | ((remoteState->b) ? B : 0) | ((remoteState->b) ? A : 0) | ((remoteState->minus) ? MINUS : 0) | (OTHER2 & 0) | (OTHER3 & 0) | ((remoteState->home) ? HOME : 0)

#define RUMBLE remoteState->rumble = *(buff+2) & 0x01
#define ACKNOWLEDGE if (buff[2] & 0x01){remoteState->lastMode = remoteState->mode; remoteState->mode = 0x22; fprintf(stderr, "Send acknowlege next input report\n"); }//Send acknowledge output report next input report
#define RESULT remoteState->result = 0

bool skipWrite = false;

struct state
	{
	bool a;
	bool b;
	bool plus;
	bool minus;
	bool home;
	bool one;
	bool two;
	bool dpadL;
	bool dpadR;
	bool dpadU;
	bool dpadD;

	bool rumble;
	bool continuous;
	bool changed;
	uint8_t mode;//data reporting mode
	uint8_t lastMode;//previous data reporting mode
	uint8_t battery;//battery level
	uint8_t led;
	bool ir;
	bool speaker;

	uint8_t eeprom[16000+1];//16000 bytes = 128kbit
	uint8_t data[16];
	uint8_t size;//number of bytes being returned
	unsigned addressBytes;//the 2 least significant bytes of the absolute memory address of the first byte of data returned

	uint8_t result;//error code or function result
	};

struct state newState;

void initState(struct state *remoteState)
	{
	remoteState->a = false;
	remoteState->b = false;
	remoteState->plus = false;
	remoteState->minus = false;
	remoteState->one = false;
	remoteState->two = false;
	remoteState->dpadL = false;
	remoteState->dpadR = false;
	remoteState->dpadU = false;
	remoteState->dpadD = false;

	remoteState->rumble = false;
	remoteState->continuous = false;
	remoteState->changed = true;
	remoteState->mode = 0x30;//data reporting mode is 0x30 by default
	remoteState->lastMode = 0;//required to handle acknowledge output report mode
	remoteState->battery = 142; //142 will do
	remoteState->led = 0x10; //LED 1 is lit by default
	remoteState->ir = false; //IR is disabled by default
	remoteState->speaker = false;
	remoteState->result = 0;

	remoteState->size = 0;

	memset(remoteState->eeprom,0,sizeof(remoteState->eeprom));//zero out the eeprom (as that's the default state)
	/* copy the default 64 calibration bytes to the eeprom*/
	memcpy(remoteState->eeprom, "\xa1\xaa\x8b\x99\xae\x9e\x78\x30\xa7\x74\xd3\xa1\xaa\x8b\x99\xae\x9e\x78\x30\xa7\x74\xd3\x82\x82\x82\x15\x9c\x9c\x9e\x38\x40\x3e\x82\x82\x82\
		\x15\x9c\x9c\x9e\x38\x40\x3e\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 64);

	memset(remoteState->data,0,16);
	}

int dataRecieved(uint8_t *buff, int length, struct state *remoteState)
	{
	/* In every single Output Report, bit 0 (0x01) of the first byte controls the Rumble feature. */
	int dataLength;//todo: relocate

	fprintf(stderr, "Recieved: %02x (%02x)", *buff, *(buff+1));
	for (int i = 2; i < length; ++i){fprintf(stderr," %02x",*(buff+i));}
	fprintf(stderr, ", Length: %d\n", length);

	if (*buff != 0xa2){fprintf(stderr,"Wii sent output report with first byte %c?\n", *buff);}

	switch (*(buff+1))
		{
		/* rumble (a2 10 RR)*/
		case 0x10:
			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		/* LED control (a2 11 LL) */
		case 0x11:
			remoteState->led = *(buff+2);

			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		/* a2 12 TT MM : TT sets whether continuous reporting is set and MM is the mode to go into */
		case 0x12:
			remoteState->continuous = *(buff+2);
			remoteState->mode = *(buff+3);
			/* Set that the remote state has been changed */
			remoteState->changed = true;

			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		/* camera reporting mode 0 (a2 13 04) */
		case 0x13:
			remoteState->ir = *(buff+2) & 0x4;//meant to be 1 of 2, but whatever

			fprintf(stderr, "IR camera enable step 1 of 2.\n");
			RUMBLE;
//			ACKNOWLEDGE;
			RESULT;
			break;

		/* camera reporting mode 1 */
		case 0x1a:
			remoteState->ir = *(buff+2) & 0x4; //meant to be 2 of 2, but whatever

			fprintf(stderr, "IR camera enable step 2 of 2.\n");
			RUMBLE;
//			ACKNOWLEDGE;
			RESULT;

			break;

		/* speaker enable or disable (a2 14 04) */
		case 0x14:
			remoteState->speaker = *(buff+2) & 0x4;

			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		/* a2 15 00 will request the status report (and turn off rumble) */
		case 0x15:
			if (*(buff+2) != 0){*(buff+length) = '\0'; fprintf(stderr,"%s unexpected\n", buff+2);}
			else
				{
				remoteState->lastMode = remoteState->mode;
				remoteState->mode = 0x20; //set staus report mode
				}

			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		/* a2 16 MM FF FF FF SS DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD */
		/* The meaning of the bytes is the same as during reads, except that size can be a maximum of 16 bytes (as there is only space for that much data),
		and the actual data to write follows (the DD bytes), padded out to 16 bytes. */
		/* FF FF FF is the offset, and SS SS is the size to read in bytes (both in big-endian format). Bit 2 (0x04) of MM selects the address space.
		Clearing this bit results in reading from EEPROM Memory, while setting it results in reading from the control registers. Setting bit 3 (0x08) also works to access
		registers, but setting both results in errors. */
		/* Some kind of acknowledgement is received on Input Report 0x22. This has not been investigated yet. */
		case 0x16:
			/* setting 0x04 or 0x08 in MM reads from control registers, but setting both results in errors */
			if ((*(buff+2) & 0x04) && (*(buff+2) & 0x08))
				{
				fprintf(stderr, "Wii set both 0x04 and 0x08?\n");
				remoteState->result = 0x04;//set unknown
				RUMBLE;
				break;
				}

			RESULT;

			/* FF FF FF is the offset */
			unsigned offsetWrite = (*(buff+3) << 16) | (*(buff+4) << 8) | *(buff+5);
			/* SS is the size to write in bytes */
			int writeSize = *(buff+6);

			if (*(buff+2) & 0x04 || *(buff+2) & 0x08)
				{
				fprintf(stderr, "Register write!\n");
				fprintf(stderr, "Memory offset: 0x%x, Write length: %d\n", offsetWrite, writeSize);

				if (offsetWrite >= 0xA20000 && offsetWrite <= 0xA30009){/* Speaker settings */}

				/* Extension Controller settings and data */
				else if (offsetWrite >= 0xA40000 && offsetWrite <= 0xA400FF)
					{
					switch (offsetWrite)
						{
						/* Writing 0x55 to 0xa400f0 disables MotionPlus and also resets wiimote, our implementation is just to skip our write and go back to read */
						case 0xa400f0:
							if (writeSize == 1 && *(buff+7) == 0x55){skipWrite = true;}
							break;
						}
					}
				else if (offsetWrite >= 0xA60000 && offsetWrite <= 0xA600FF){/* Wii motion plus settings and data */}
				else if (offsetWrite >= 0xB00000 && offsetWrite <= 0xB00033){/* IR Camera settings */}
				else {fprintf(stderr,"Unknown offset %#06x requested\n", offsetWrite);}
				}
			else
				{
				fprintf(stderr, "EEPROM write!\n");
				/* EEPROM write */
				/* FF FF FF is the offset */

				memcpy(remoteState->eeprom+offsetWrite, buff+7, writeSize);

				remoteState->lastMode = remoteState->mode;
				/* confirm the data write */
				remoteState->mode = 0x22;//check
				}

			RUMBLE;
			break;

		/* Read data (a2 17 MM FF FF FF SS SS)*/
		case 0x17:
			/* setting 0x04 or 0x08 in MM reads from registers, while setting both causes an error */
			if ((*(buff+2) & 0x04) && (*(buff+2) & 0x08))
				{
				fprintf(stderr, "Remote state is unknown\n");
				remoteState->result = 0x04;//set unknown
				RUMBLE;
				break;
				}

			RESULT;

			/* FF FF FF is the offset */
			unsigned offset = (*(buff+3) << 16 ) + (*(buff+4) << 8) + *(buff+5);
			/* SS SS is the size to read in bytes */
			int size = (*(buff+6) << 8) + *(buff+7);

			remoteState->size = size;

			if (*(buff+2) & 0x04 || *(buff+2) & 0x08)
				{
				/* register read */
				fprintf(stderr, "Register read; Memory offset: 0x%x, Read length: %d\n", offset, size);

				if (offset >= 0xA20000 && offset <= 0xA30009){/* speaker settings */}
				/* Extension Controller settings and data */
				else if (offset >= 0xA40000 && offset <= 0xA400FF)
					{

					}

				/* Wii motion plus settings and data */
				else if (offset >= 0xA60000 && offset <= 0xA600FF)
				{
					switch (offset)
						{
						case 0xA600FA:
							if (size == 6)
								{
								/* If a Motion+ adaptor is available, 6 bytes of data from the address will be returned, we just return 0x07 for not attached */
								fprintf(stderr, "Return Motion+ not connected\n");
								remoteState->result = 0x07;
								remoteState->size = 0;
								}
						}
				}

				else if (offset >= 0xB00000 && offset <= 0xB00033){/* IR Camera settings */}
				else {fprintf(stderr,"Unknown Read %#06x requested\n", offset);}
				}
			else
				{
				/* EEPROM read */
				fprintf(stderr, "EEPROM read; Memory offset: 0x%x, Read length: %d\n", offset, size);

				memcpy(remoteState->data, remoteState->eeprom+offset, size);
				remoteState->size = size;
				/* pad rest with 0 if read size is not 16 */
				if (size < 16)
					{
					memset(remoteState->data+size, 0, 16-size);
					}

				}

			/* the 2 least significant bytes of the absolute memory address of the first byte of data is returned */
			remoteState->addressBytes = offset & 0xFFFF;
			remoteState->lastMode = remoteState->mode;
			remoteState->mode = 0x21;

			RUMBLE;
			break;

		/* Speaker data */
		case 0x18:
			/* length is shifted left by 3 bytes, so shift right by 3 */
			dataLength = (*(buff+2) >> 3);

			memcpy(buff,buff+3,dataLength);

			//playSound(buff);
			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;


		/* Speaker mute (a2 19 04) */
		case 0x19:
			remoteState->speaker = *(buff+2) ^ 0x04;

			RUMBLE;
			ACKNOWLEDGE;
			RESULT;
			break;

		default:
			fprintf(stderr,"Unimplemented output report: %x\n", *(buff+1));
			break;
		}

	return 0;
	}

int writeCommand(struct state *remoteState)
	{
	uint8_t buff[MAX];

	int size = 0;
	switch (remoteState->mode)
		{
		case 0x20:
			/* This mode is used when The Wiimote conforms sync by writing: (a1) 20 BB BB LF 00 00 VV to the data pipe, or by request */
			buff[0] = 0xa1;
			buff[1] = 0x20;
			buff[2] = 0x20;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			buff[4] = remoteState->led;
			buff[5] = 0;
			buff[6] = 0;
			buff[7] = remoteState->battery;

			/* Go back to previous mode */
			remoteState->mode = remoteState->lastMode;

			size = 8;
			break;

		/* Read Memory Data - returns 1 to 16 bytes at a time */
		/* (a1) 21 BB BB SE AA AA DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD DD */
		case 0x21:
			buff[0] = 0xa1;
			buff[1] = 0x21;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;

			/* If return is last return or bytes to return are 16 or less, change back to previous mode */
			if (remoteState->size <= 16)
				{
				remoteState->mode = remoteState->lastMode;
				/* S (high nybble of SE) is the size in bytes, minus one, for the current data packet.
				This is 0xf (16 bytes) for all but the last packet, where it might be less if the requested number of bytes is not a multiple of 16. */

				/* E (low nybble of SE) is the error flag. 0 for no error, 7 when attempting to read a write-only register or non-connected expansion
				and 8 when attempting nonexistant memory addresses read. */
				buff[4] = ((remoteState->size-1 << 4) & 0xF0) | (remoteState->result & 0x0F);//looks about right (check)
				}

			else 	{
				buff[4] = ((16-1 << 4) & 0xF0) | (remoteState->result & 0x0F);//looks about right (check)
				}

			/* AA AA are the 2 least significant bytes of the absolute memory address of the first byte of data returned */
			/* AA AA increases by 16 each time each 16 byte memory read */
			buff[5] = remoteState->addressBytes & 0xFF00;
			buff[6] = remoteState->addressBytes & 0x00FF;
			remoteState->addressBytes += 16;

			memcpy(buff+7, remoteState->data, 16);

			/* as we are just about to return 16 bytes, reduce total count by 16 (less than 16 bytes return possible, but value isn't used then) */
			remoteState->size -= 16;

			size = 23;
			break;

		/* Acknowledge output report, return function result (It is sent when bit 1 of the first byte of any output report is set.): a1 22 BB BB RR EE */
		case 0x22:
			buff[0] = 0xa1;
			buff[1] = 0x22;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			buff[4] = remoteState->lastMode;//the output report to confirm
			buff[5] = remoteState->result; //the returned result
			size = 6;

			fprintf(stderr, "Acknowlegment sent: %02x %02x %02x %02x %02x %02x\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6]);
			remoteState->mode = remoteState->lastMode;//revert mode
			break;

		/* Core buttons */
		case 0x30:
			if (remoteState->changed){remoteState->changed = false; skipWrite = true;}

			SDL_PumpEvents();
			const uint8_t *keyBoardState = SDL_GetKeyboardState(NULL);//get keyboard state
			if (keyBoardState[SDL_SCANCODE_A]){fprintf(stderr, "A pressed\n"); remoteState->a = true; skipWrite = false;}
			else {remoteState->a = false;}

			buff[0] = 0xa1;
			buff[1] = 0x30;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;

			size = 4;
			break;

		/* Core buttons and Accelerometer */
		case 0x31:
			buff[0] = 0xa1;
			buff[1] = 0x31;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* 0x80 for approx. stillness */
			buff[4] = 0x80;//X axis(?)
			buff[5] = 0x80;//Y axis(?)
			buff[6] = 0x80;//Z axis(?)

			size = 7;
			break;

		/* Core buttons with 8 extension bytes */
		case 0x32:
			buff[0] = 0xa1;
			buff[1] = 0x32;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* no extension support...yet, so just send all 0 */
			memset(buff+4,0,8);

			size = 12;
			break;

		/* Core buttons and accelerometer with 12 IR bytes */
		case 0x33:
			buff[0] = 0xa1;
			buff[1] = 0x33;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* 0x80 for approx. stillness */
			buff[4] = 0x80;//X axis(?)
			buff[5] = 0x80;//Y axis(?)
			buff[6] = 0x80;//Z axis(?)
			/* 12 IR bytes, all 0 since no IR...yet */
			memset(buff+7,0,12);

			size = 19;
			break;

		/* core buttons with 19 extension bytes */
		case 0x34:
			buff[0] = 0xa1;
			buff[1] = 0x34;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* 19 extension bytes, all 0, since no IR...yet */
			memset(buff+4,0,19);

			size = 23;
			break;

		/* Core buttons and Accelerometer with 16 extension bytes */
		case 0x35:
			buff[0] = 0xa1;
			buff[1] = 0x35;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* 16 extension bytes, all 0, since no extensions...yet */
			memset(buff+4,0,16);

			size = 20;
			break;

		/* Core buttons with 10 IR bytes and 9 extension bytes */
		case 0x36:
			buff[0] = 0xa1;
			buff[1] = 0x36;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* 10 IR bytes */
			memset(buff+4,0,10);
			/* 9 extension bytes */
			memset(buff+14,0,9);

			size = 23;
			break;

		case 0x37:
			/* Core buttons and Accelerometer with 10 IR bytes and 6 extension bytes */
			buff[0] = 0xa1;
			buff[1] = 0x37;
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;
			/* Accelerometer */
			buff[4] = 0x80;//X axis(?)
			buff[5] = 0x80;//Y axis(?)
			buff[6] = 0x80;//Z axis(?)
			/* 10 IR bytes */
			memset(buff+7,0,10);
			/* 6 extension bytes */
			memset(buff+17,0,6);

			size = 23;
			break;

		/* 21 extension bytes */
		case 0x3d:
			buff[0] = 0xa1;
			buff[1] = 0x3d;
			memset(buff+2,0,21);

			size = 23;
			break;

		/* Interleaved 0 - core buttons and accelerometer with 18 IR bytes */
		case 0x3e:
			buff[0] = 0xa1;
			buff[1] = 0x3e;
			/* todo: store Z values in unused bits */
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;

			buff[4] = 0x80;//XX somewhat still(?)
			memset(buff+5,0,18);

			size = 23;
			break;

		/* Interleaved 1 - core buttons and accelerometer with 18 IR bytes */
		case 0x3f:
			buff[0] = 0xa1;
			buff[1] = 0x3f;
			/* todo: store Z values in unused bits */
			buff[2] = LEFT_B;
			buff[3] = RIGHT_B;

			buff[4] = 0x80;//YY somewhat still(?)
			memset(buff+5,0,18);

			size = 23;
			break;

		default:
			fprintf(stderr,"State: %hi not implemented\n", remoteState->mode);
			break;
		}

	fprintf(stderr, "Sent: %02x (%02x)", buff[0], buff[1]);
	for (int i = 2; i < size; ++i){fprintf(stderr," %02x",buff[i]);}
	fprintf(stderr, ", Length: %d\n", size);

	return write(dataSock, buff, size);
	}

#endif