#define F_CPU 7372800

#include <inttypes.h>

#include <string.h>
#include "uart0.h"
#include "uart1.h"
#include "printf-stdarg.h"

#include "web_cam.h"

// global varibles that get used...
uint8_t command[6]; // the command buffer
int PACKAGE_SIZE = 128; // the packet size to use for jpeg

// the delay to use in receive timeout
#define RXDELAY 30                // the delay for most command packets
#define PACKAGE_DELAY 1000        // the delay for actual jpeg package transfer
// FIFO Buffer and pointers
#define FIFOSIZE 128              // the FIFO serial buffer size (circular)
volatile uint8_t bytesin = 0;
volatile uint8_t writeblock = 0;
volatile uint8_t readblock = 0;
volatile uint8_t overflow = 0;
volatile uint8_t rxfifo[FIFOSIZE];

/*--------------Serial Routines------------------*/
#define USART1_BAUDRATE 115200  //14400
#define BAUD1_PRESCALE (((F_CPU / (USART1_BAUDRATE * 16UL))) - 1)

void uart1_tx(uint8_t buffer[], int size) {
	for (int count = 0; count < size; count++) {
		while(uart1_SendChar(buffer[count])!=ERR_OK);
	}
}


int uart1_rx(volatile uint8_t buffer[], int size, unsigned int timeout) {
	word count;
	uart1_RecvBlock(buffer,size,&count,timeout);
	return count;
}

/*--------------Camera Routines------------------*/
int get_ack(uint8_t expected_ack) {
	uint8_t buffer[6];
	if (uart1_rx(buffer, 6, RXDELAY) == -1) {
		xprintf("ACK Timed out!\n");
		return -1;
	};

	// If NAK is received instead of ACK - raise exception
	if (buffer[1] == CMD_NAK) {
		xprintf("C328R Error - NAK\n");
		return -1;
	}
	// If no ACK or ACK for different command received - return false
	if (buffer[1] != CMD_ACK || buffer[2] != expected_ack) {
		return -1;
	}
	return 1;
}


int sync_camera() {
	portTickType =LastWakeUpTime;
	uint8_t recv_command[6] = { 0, 0, 0, 0, 0, 0 };
	int connected = -1;

	// Create 'Sync' command
	command[0] = CMD_PREFIX;
	command[1] = CMD_SYNC;
	command[2] = 0;
	command[3] = 0;
	command[4] = 0;
	command[5] = 0;

	Last

	for (int i = 0; i < 60; i++) {
		if (connected == 1) {
			break;
		}

		uart1_tx(command, 6);
		vTaskDelay(5/portTICK_RATE_MS);// 5ms Pauza medzi synchronizaciami

		// Wait for ACK
		uart1_rx(recv_command, 6, RXDELAY);
		if (recv_command[1] != CMD_ACK || recv_command[2] != CMD_SYNC) {
			continue;
		}

		// Wait for SYNC
		uart1_rx(recv_command, 6, RXDELAY);
		if (recv_command[1] == CMD_SYNC) {
			connected = 1;
		} else {
			continue;
		}

		// Create 'ACK' command
		command[0] = CMD_PREFIX;
		command[1] = CMD_ACK;
		command[2] = CMD_SYNC;
		command[3] = 0;
		command[4] = 0;
		command[5] = 0;

		uart1_tx(command, 6);
	}
	return connected;
}

int init_camera(uint8_t color_type, uint8_t preview_resolution,
		uint8_t jpeg_resolution) {
	// Create the command
	command[0] = CMD_PREFIX;
	command[1] = CMD_INITIAL;
	command[2] = 0;
	command[3] = color_type;
	command[4] = preview_resolution;
	command[5] = jpeg_resolution;

	// send 'Initial' command
	uart1_tx(command, 6);

	// Wait for ACK
	if (get_ack(CMD_INITIAL) == -1) {
		return -1;
	}

	// Create package size command

	command[0] = CMD_PREFIX;
	command[1] = CMD_PACKAGESIZE;
	command[2] = 0x08;
	command[3] = (uint8_t) PACKAGE_SIZE; // PACKAGE_SIZE Low byte
	command[4] = (uint8_t) (PACKAGE_SIZE >> 8); // PACKAGE_SIZE High byte
	command[5] = 0x00;

	// Send 'Set Package Size' command
	uart1_tx(command, 6);

	// Wait for ACK
	if (!get_ack(CMD_PACKAGESIZE)) {
		return -1;
	}

	return 1;
}

int poweroff_camera() {
	// Create the command
	command[0] = CMD_PREFIX;
	command[1] = CMD_POWEROFF;
	command[2] = 0;
	command[3] = 0;
	command[4] = 0;
	command[5] = 0;

	// send 'Initial' command
	uart1_tx(command, 6);

	// Wait for ACK
	if (get_ack(CMD_INITIAL) == -1) {
		return -1;
	}

	return 1;
}

int reset_camera(uint8_t reset_type) {
	// Create the command
	command[0] = CMD_PREFIX;
	command[1] = CMD_RESET;
	command[2] = reset_type;
	command[3] = 0;
	command[4] = 0;
	command[5] = 0;

	// send 'Initial' command
	uart1_tx(command, 6);

	// Wait for ACK
	if (get_ack(CMD_INITIAL) == -1) {
		return -1;
	}

	return 1;
}

int set_snapshot(uint8_t snapshot_type) {
	// Create the command
	command[0] = CMD_PREFIX;
	command[1] = CMD_SNAPSHOT;
	command[2] = snapshot_type;
	command[3] = 0;
	command[4] = 0;
	command[5] = 0;

	// send 'Snapshot' command
	uart1_tx(command, 6);

	// Wait for ACK
	if (!get_ack(CMD_SNAPSHOT)) {
		return -1;
	}
	return 1;
}

uint32_t take_picture(uint8_t picture_type) {
	uint32_t picture_data_size = 0;

	// Create 'Take Picture' command
	command[0] = CMD_PREFIX;
	command[1] = CMD_GETPICTURE;
	command[2] = picture_type;
	command[3] = 0x00;
	command[4] = 0x00;
	command[5] = 0x00;

	// Send 'Take Picture' command
	uart1_tx(command, 6);

	// Give camera time to process the image
	_delay_ms(200);

	// Receive ACK
	if (get_ack(CMD_GETPICTURE) == -1) {
		return 0;
	}

	// Receive DATA command, with information about image
	uint8_t response[6];
	uart1_rx(response, 6, RXDELAY);
	if (response[1] != CMD_DATA) {
		return 1;
	}

	// Get dataSize from three bytes
	picture_data_size = response[5] << 8;
	picture_data_size |= response[4] << 8;
	picture_data_size |= response[3];

	return picture_data_size;
}

void clearbuffer(volatile uint8_t buffer[], int size) {
	for (int x = 0; x < size; x++)
		buffer[x] = '0';
}

int get_jpeg_data(int data_length) {
	int ret = 1;

	// init data page
	volatile uint8_t picture_data[PACKAGE_SIZE];
	int package_counter = 0;

	int total_packages = (data_length / (PACKAGE_SIZE - 6));
	if ((total_packages * (PACKAGE_SIZE - 6)) < data_length)
		total_packages++; // make sure to round up, not down

	//xprintf("DataLength: %d\n",data_length);
	//xprintf("%d packages %d bytes each is %d bytes\n",total_packages,PACKAGE_SIZE-6,total_packages*(PACKAGE_SIZE-6));

	xprintf("ZZZZZZZZZZ"); // trigger terminal pic capture
	xprintf("%d\n", data_length); // the total data length
	xprintf("%d\n", PACKAGE_SIZE - 6); // the packet size to expect

	while (total_packages > package_counter) // first packet is 0, so we should be 1 short
	{
		// prepare ack packet
		command[0] = CMD_PREFIX;
		command[1] = CMD_ACK;
		command[2] = 0;
		command[3] = 0;
		command[4] = package_counter; // The packet requested from the camera
		command[5] = (package_counter >> 8);

		// clear the response array
		clearbuffer(picture_data, PACKAGE_SIZE);

		// send ack to start the data package return
		uart1_tx(command, 6);

		// get one package back
		_delay_ms(1);
		int got = uart1_rx(picture_data, PACKAGE_SIZE, PACKAGE_DELAY);
		int packet_size = 0;
		if (got > 4) {
			packet_size = picture_data[3] << 8;
			packet_size |= picture_data[2];
		}

		// Check the CRC of the incoming packet with the verify byte at the end, set a flag if we need to try again
		int crc = 0;
		int badcrc = 0;
		for (int x = 0; x < packet_size + 4; x++) // all bytes of the packet except the verify 2 bytes
			crc += picture_data[x];
		if ((crc & 0xFF) == picture_data[packet_size + 4])
			badcrc = 0;
		else
			badcrc = 1;

		//xprintf("For package %d we got %d bytes and a packet size of %d\n",package_counter,got,packet_size);

		if (((got - 6) < packet_size) || (badcrc == 1)) // failed packet or CRC, re-request it
		{
			// if we lost a byte, lets realign the buffer/pointers and allow it to try again
			// the bug, which I don't understand how is happening causes bytesin to be 0 while there is still a byte
			// in the buffer which is seen via read and write not lining up.
			// If we didn't loose a byte, since we don't increment the ID it will rerequest the packet for the failed crc
			if ((got - 6) < packet_size) {
				readblock = 0;
				writeblock = 0;
				bytesin = 0;
			}
		} else { // successful packet, now send out to terminal
			package_counter++;

			for (int x = 4; x < packet_size + 4; x++) //relay the data one byte at a time in hex to the terminal app
			{ //binary seems more error prone, needs work
				//xprintf("%02X ",picture_data[x]);
				while (!(UCSR0A & (1 << UDRE0)))
					;
				UDR0 = picture_data[x];
			}
		}
	}

	// send final package ACK
	command[0] = CMD_PREFIX;
	command[1] = CMD_ACK;
	command[2] = 0;
	command[3] = 0;
	command[4] = 0xF0; //per the spec, F0 on each of these signifies we are done
	command[5] = 0xF0;

	uart1_tx(command, 6);
	//xprintf("ZZZZZZZZZZ");     // end signature to terminal app

	return ret;
}
