#include "effect_tpm2_net.h"
#include "my_fastled.h"

void Tpm2NetEffect::loop(uint16_t ms) {
	int packetsize = _udp.parsePacket();
	uint8_t data[255];
	uint8_t type = 0x00;
	if (packetsize > 0) {
		_last_packet_at = millis();
		DBG("TPM2.Net * Received %d bytes from %s", packetsize, _udp.remoteIP().toString().c_str());
		if (packetsize < 7) {
			LOGln("TPM2.Net * Packet is too short. Ignoring it.");
			return;
		}
		_udp.read(data, 6);
		if (data[0] != 0x9C) {
			LOGln("TPM2.Net * Block start byte is 0x%02X, expected 0x9C", data[0]);
			return;
		}
		if (data[1] == 0xC0) {
			DBG("TPM2.Net * Received a command packet.");
			type = 0xC0;
		} else if (data[1] == 0xDA) {
			DBG("TPM2.Net * Received a data packet.");
			type = 0xDA;
		} else {
			LOGln("TPM2.Net * Unexpected packet type 0x%02X, expected 0xC0 or 0xDA.", data[1]);
		}
		uint16_t framesize = (data[2]<<8) | data[3];
		uint8_t packet_number = data[4];
		//uint8_t packet_count = data[5];
		
		if (packetsize != framesize + 7) {
			LOGln("TPM2.Net * Invalid packet size. Expected %d bytes, was %d bytes.", framesize+7, packetsize);
			return;
		}
		
		if (type==0xC0) {
			_parse_command(framesize, packet_number);
		} else if (type==0xDA) {
			_parse_data(framesize, packet_number);
		}
	} else if (_last_packet_at + 5000 < millis()) {
		window->fill_with_checkerboard();
	}
}

void Tpm2NetEffect::_parse_command(uint16_t size, uint8_t packet_number) {
	if (packet_number!=0) {
		LOGln("TPM2.Net * Command packet with packet_number > 0 (%d). Ignoring it.", packet_number);
		return;
	}
	if (size < 2) {
		LOGln("TPM2.Net * Command packet need at least 2 data bytes.");
		return;
	}
	uint8_t ctrl = _udp.read();
	bool write = ctrl & 0x80;
	bool respond = ctrl & 0x40;
	uint8_t cmd = _udp.read();
	uint8_t data = 0;
	if (write) {
		if (size<3) {
			LOGln("TPM2.Net * Got a write command, but no data to write.");
			return;
		}
		data = _udp.read();
	}
	
	if (cmd == 0x0A) { // Master Brightness
		if (write) {
			FastLED.setBrightness(data);
		} else {
			uint8_t data[1] = {FastLED.getBrightness()};
			_respond_with_data(data, 1);
		}
	} else if (cmd == 0x10 && !write) {
		uint16_t pixels = window->width * window->height;
		uint8_t data[2] = {(uint8_t)(pixels >> 8), (uint8_t)(pixels & 0xFF)};
		_respond_with_data(data, 2);
	} else {
		LOGln("TPM2.Net * Unknown command. write:%d, command:0x%02X", write, cmd);
		if (respond) {
			_respond_unknown_command();
		}
	}
}

void Tpm2NetEffect::_parse_data(uint16_t framesize, uint8_t packet_number) {
	if (packet_number==0) {
		_pixel_index=0;
	}
	uint8_t len;
	uint8_t data[3];
	CRGB color;
	while ((len = _udp.read(data, 3))==3) {
		// We got 3 bytes
		color.setRGB(data[0], data[1], data[2]);
		window->setPixelByIndex(_pixel_index, &color);
		_pixel_index++;
	}

	/*
	bool dir_changed = false;
	
	_x += _x_dir * settings.effects.dvd.speed;
	_y += _y_dir * settings.effects.dvd.speed;

	if (_x <= 0) {
		_x = -_x;
		_x_dir = -_x_dir;
		dir_changed = true;
		//LOGln("speed: %d", settings.effects.dvd.speed);
	} else if (_x + (settings.effects.dvd.width << 8) >= (window->width << 8)) {
		_x -= 2*settings.effects.dvd.speed;
		_x_dir = -_x_dir;
		dir_changed = true;
	}
	
	if (_y <= 0) {
		_y = -_y;
		_y_dir = -_y_dir;
		dir_changed = true;
	} else if (_y + (settings.effects.dvd.height << 8) >= (window->height << 8)) {
		_y -= 2*settings.effects.dvd.speed;
		_y_dir = -_y_dir;
		dir_changed = true;
	}

	window->clear();
	
	if (dir_changed) _color = (CRGB)CHSV(random8(), 255, 255);

	for (int x=0; x<settings.effects.dvd.width; x++) for (int y=0; y<settings.effects.dvd.height; y++) {
		window->setSubPixel(_x + (x<<8), _y + (y<<8), (CRGB*)&_color);
	}
	for (int x=1; x<settings.effects.dvd.width; x++) for (int y=1; y<settings.effects.dvd.height; y++) {
		window->setPixel((_x>>8) + x, (_y>>8) + y, (CRGB*)&_color);
	}
	*/
}

void Tpm2NetEffect::_respond(uint8_t* data, uint8_t len) {
	_udp.beginPacket(_udp.remoteIP(), 65442);
	_udp.write(data, len);
	_udp.endPacket();
}

void Tpm2NetEffect::_respond_ack() {
	uint8_t data[8] = {0x9C, 0xAC, 0x00, 0x01, 0x00, 0x01, 0x00, 0x36};
	_respond(data, 8);
}

void Tpm2NetEffect::_respond_unknown_command() {
	uint8_t data[8] = {0x9C, 0xAC, 0x00, 0x01, 0x00, 0x01, 0x02, 0x36};
	_respond(data, 8);
}

void Tpm2NetEffect::_respond_with_data(uint8_t* dat, uint8_t len) {
	uint8_t data[8 + len];
	data[0] = 0x9C;
	data[1] = 0xAD;
	data[2] = (len+1)>>8;
	data[3] = (len+1)&0xFF;
	data[4] = 0x00;
	data[5] = 0x01;
	data[6] = 0x00;
	memcpy(&(data[7]), dat, len);
	data[8 + len - 1] = 0x36;
	_respond(data, 8 + len);
}

bool Tpm2NetEffect::can_be_shown_with_clock() { return false; }

Tpm2NetEffect::Tpm2NetEffect() {
	_udp.begin(65506);
}

Tpm2NetEffect::~Tpm2NetEffect() {
	_udp.stop();
}