#define F_CPU 7372800

#include <inttypes.h>

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

#include "webcam.h"

// the delay to use in receive timeout
#define RXDELAY 30               // the delay for most command packets
#define PACKAGE_DELAY 50        // 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];

// global varibles that get used...
uint8_t command[6]; // the command buffer

int PACKAGE_SIZE = 128; // the packet size to use for jpeg
int akt_package_counter = 0; //Globlana premena pre aktualny paket, pouziva sa pri http nacitavanie s kamery
int akt_total_packages = 0;
int start_package_counter = 0;
int last_length = 4;
int next_length = 4;
unsigned long cam_data_Length;

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

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

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

/*--------------Camera Routines------------------*/
int get_ack(uint8_t expected_ack) {
	uint8_t buffer[6];
	if (uart0_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;
}

/*--------------Camera Routines------------------*/
int get_ack_customDelay(uint8_t expected_ack, const uint16 maxDelay) {
	uint8_t buffer[6];
	if (uart0_rx(buffer, 6, maxDelay) == -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;
	int i;

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

	LastWakeUpTime = xTaskGetTickCount();
	kameraBit_SetVal();
	for (i = 0; i < 600; i++) {

		if ((i == 60) || (i == 120)) {
			kameraBit_ClrVal();
			vTaskDelay(50);
			kameraBit_SetVal();
		}

		if (connected == 1) {
			break;
		}

		uart0_tx(command, 6);
		vTaskDelayUntil(&LastWakeUpTime, 5 / portTICK_RATE_MS);// 10ms Pauza medzi synchronizaciami

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

		// Wait for SYNC
		uart0_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;

		uart0_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
	uart0_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
	uart0_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
	uart0_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] = 0xff;

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

	// Wait for ACK
	if (get_ack_customDelay(CMD_INITIAL, 1000) == -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
	uart0_tx(command, 6);

	// Wait for ACK
	if (!get_ack_customDelay(CMD_SNAPSHOT, 1000)) {
		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
	uart0_tx(command, 6);

	// Give camera time to process the image
	//vTaskDelay(100);

	// Receive ACK
	if (get_ack_customDelay(CMD_GETPICTURE, 1000) == -1) {
		return 0;
	}

	// Receive DATA command, with information about image
	uint8_t response[6];
	uart0_rx(response, 6, 500);
	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) {
	int x;
	for (x = 0; x < size; x++)
		buffer[x] = '0';
}
#ifdef camtest
int get_jpeg_data(int data_length) {
	int ret = 1;
	int x;

	// 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("\n\rZZZZZZZZZZ"); // trigger terminal pic capture
	xprintf("\n\r%d", data_length); // the total data length
	xprintf("\n\r%d", 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
		uart0_tx(command, 6);

		// get one package back

		int got = uart0_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 (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 (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])
				;

			}
		}
	}

	// 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;

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

	return ret;
}
#endif
/*
 **==========================================================================================
 ** Function: get_jpeg_data_packet
 ** Arguments: char*, int
 ** Return:  int
 ** Comment: naciatanie n bajtov z kamery, vyuziva globalne premenne a makra cam_xxx
 **===========================================================================================
 */

int get_jpeg_data_packet(char* buffer, int length) {
	int ret = -1;
	int x, xBuff = 0;
	int last_length_internal;

	// init data page
	volatile uint8_t picture_data[PACKAGE_SIZE];

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

	//xprintf("\n\rZZZZZZZZZZ"); // trigger terminal pic capture
	//xprintf("\n\r%d \n\r", PACKAGE_SIZE - 6); // the packet size to expect

	akt_package_counter = start_package_counter;
	last_length_internal = last_length;
#ifdef debugKam
	xprintf("\n\rPart of image");
#endif
	while (ret == -1) {

		if (akt_total_packages > akt_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] = akt_package_counter; // The packet requested from the camera
			command[5] = (akt_package_counter >> 8);

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

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

			// get one package back
			int got = uart0_rx(picture_data, PACKAGE_SIZE, PACKAGE_DELAY);
			if (got == 0)
				return -1;

			clearbuffer(picture_data + got, PACKAGE_SIZE - got);
			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 (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
				//critical_ERROR_WRITE("CHYBA CRC ale PACKETU");
				if ((got - 6) < packet_size) {
					readblock = 0;
					writeblock = 0;
					bytesin = 0;
				}
			} else { // successful packet, now send out to terminal
				akt_package_counter++;
				//xprintf("AKTUALNY PAKET: %d\n\r",akt_package_counter);
				for (x = last_length_internal; 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]);
					if (length > 0) {
						buffer[xBuff++] = picture_data[x];
						//xprintf("%02X ",picture_data[x]);

					} else {
						if (x < packet_size + 4)
							next_length = x;
						else
							next_length = 4;

						return xBuff;

					}
					length--;
				}
				last_length_internal = 4;

				/****START OF BLOCK: pokus o zrychlenie, zrychlenie male ak vobec dake a je tu chyba*****************************************/
				/*crc = packet_size;//aby som nemusel novu premenu tvorit
				 if (packet_size > length)
				 packet_size = length;
				 for (x = last_length_internal; 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]);
				 buffer[xBuff++] = picture_data[x];

				 }

				 length -= (x - last_length_internal);


				 if (length <= 0) {
				 if (x < crc + 4)
				 next_length = x;
				 else
				 next_length = 4;

				 return xBuff;

				 }

				 last_length_internal = 4;*/
				//----END OF BLOCK: pokus o zrychlenie, zrychlenie male ak vobec dake a je tu chyba------------------------------------------


			}
		} else {
			return xBuff;
		}
	}

	return ret;
}
/*--END OF FUNCTION: get_jpeg_data_packet-----------------------------------------------------*/

/*
 **==========================================================================================
 ** Function: get_jpeg_data_packet_end
 ** Arguments: void
 ** Return:  void
 ** Comment: posle ukoncovaci prikazz pre komunikaciu s kamerou
 **===========================================================================================
 */
void get_jpeg_data_packet_end(void) {
	// 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;

	uart0_tx(command, 6);
	//xprintf("ZZZZZZZZZZ");     // end signature to terminal app
}
/*--END OF FUNCTION: get_jpeg_data_packet_end-----------------------------------------------------*/
/*
 ** ===================================================================
 **     Method      :  kameraBit_PutVal (component BitIO)
 **
 **     Description :
 **         This method writes the new output value.
 **     Parameters  :
 **         NAME       - DESCRIPTION
 **         Val             - Output value. Possible values:
 **                           FALSE - logical "0" (Low level)
 **                           TRUE - logical "1" (High level)
 **     Returns     : Nothing
 ** ===================================================================
 */
void kameraBit_PutVal(bool Val) {
	if (Val) {
		setReg8(SETTA, 0x02); /* SETTA1=0x01 */
	} else { /* !Val */
		setReg8(CLRTA, 0xFD); /* CLRTA1=0x00 */
	} /* !Val */
}

/*
 **==========================================================================================
 ** Function: picture_taker
 ** Arguments: void*
 ** Return:  void
 ** Comment: task, ktory do nekonecna synchronizuje sa s kamerou
 **===========================================================================================
 */
void picture_taker(void *pvParameters) {
	portTickType previousTime = xTaskGetTickCount();
	for (;;) {

		xSemaphoreTake(CamPictureMutex,portMAX_DELAY);
#ifdef debugKam
		xprintf("\n\rZac");
#endif
		/****START OF BLOCK: Obrazok z kamery !!*****************************************/
		reset_camera(RESET_STATES);
		vTaskDelay(5);
		/****START OF BLOCK: Synchonizacia s kamerou*****************************************/
#ifdef debugFS
		xprintf("\n\rZaciatok synchronizacie");
#endif
		if (sync_camera() == 1) {
#ifdef debugFS
			xprintf("\n\rKam pripojena")
			;
#endif
		} else {
#ifdef debugFS
			xprintf("\n\rChyba sync kamery")
			;
#endif
		}
		//----END OF BLOCK: Synchonizacia s kamerou------------------------------------------
		vTaskDelay(10);
		/****START OF BLOCK: Inicializacia kamery*****************************************/
		if (init_camera(COLOR_JPEG, PREVIEW_R160x120, JPEG_R320x240) == 1) {
#ifdef debugFS
			xprintf("\n\rKamera inicializovana")
			;
#endif
		} else {
#ifdef debugFS
			xprintf("\n\r Chyba inicializacie");
#endif
			break;
		}
		//----END OF BLOCK: Inicializacia kamery------------------------------------------
		vTaskDelay(10);
		/****START OF BLOCK: Zachytenie obrazku*****************************************/
		if (set_snapshot(SNAPSHOT_COMPRESSED) == -1) {
#ifdef debugFS
			xprintf("\n\rChyba SNAMPSHOT")
			;
#endif
		} else {
#ifdef debugFS
			xprintf("\n\rSNAMPSHOT OK")
			;
#endif
		}
		vTaskDelay(10);
		cam_data_Length = take_picture(TYPE_SNAPSHOT);
#ifdef debugFS
		xprintf("\n\r Velkost obrazka: %d",cam_data_Length);
#endif
		//----END OF BLOCK: Zachytenie obrazku------------------------------------------

		xSemaphoreGive(CamPictureMutex);
#ifdef debugKam
		xprintf("\n\rKon");
#endif
		vTaskDelayUntil(&previousTime,200); // Ide spat na 150ms

		//----END OF BLOCK: Obrazok z kamery !!------------------------------------------
	}
}
/*--END OF FUNCTION: picture_taker-----------------------------------------------------*/
