#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "st7735.h"

/********************************** EASY PORT *********************************/
/*
 * If you porting this code, you can change below headers and function pointers
 * in gpio structure.
 */
#include <wiringPi.h>
#include <wiringPiSPI.h>
struct
{
	void (* const delay)(unsigned int milliseconds);
	void (* const pinMode)(int pin, int mode);
	void (* const digitalWrite)(int pin, int value);
	int  (* const spiSetup)(int channel, int speed);
	int  (* const spiDataRW)(int channel, uint8 *data, int length);
} static const gpio =
{
	delay,
	pinMode,
	digitalWrite,
	wiringPiSPISetup,
	wiringPiSPIDataRW
};
/****************************** END EASY PORT END *****************************/

static uint8 screen_buffer[SCREEN_HEIGHT * SCREEN_WIDTH * 3]; // TODO: make dynamic
static uint16 screen_window_x1;
static uint16 screen_window_x2;
static uint16 screen_window_y1;
static uint16 screen_window_y2;
static uint16 screen_cursor_x;
static uint16 screen_cursor_y;

void advance_screen_cursor()
{
	screen_cursor_x++;
	if (screen_cursor_x > screen_window_x2) {
		screen_cursor_x = screen_window_x1;
		screen_cursor_y++;
		if (screen_cursor_y > screen_window_y2) {
			screen_cursor_y = screen_window_y1;
		}
	}
}

static lcd_t *activeDisplay;

/*
 * Safe allocation of the memory block.
 *
 * Parameters:
 *   size - Size of memory block to allocate.
 *
 * Return:
 *   Pointer to the memory block. If an error occurs, stop the program.
 */
static inline void *safeMalloc(size_t size)
{
	void *memoryBlock = (void*) malloc(size);

	/* Check the pointer */
	if(memoryBlock == NULL)
	{
		fprintf(stderr, "Out of RAM memory!\n");
		exit(EXIT_FAILURE);
	}
	
	return memoryBlock;
} /* safeMalloc */


uint16 lcdhw_setWindow(lcd_t* lcd, uint16 x1, uint16 y1, uint16 x2, uint16 y2);
void lcdhw_pushPixel(lcd_t* lcd, uint8 r, uint8 g, uint8 b);
void lcdhw_pushPixels(lcd_t* lcd, uint8* pixels, size_t count);

void lcd_setOrientation(lcd_t* lcd, uint16 orientation);
void lcd_setGamma(lcd_t* lcd, uint16 state);
void lcd_pushPixel(lcd_t* lcd, uint8 r, uint8 g, uint8 b);
void lcd_pushPixels(lcd_t* lcd, uint8* pixels, size_t count);

/*
 * Write the command to the display driver.
 *
 * Parameters:
 *   cmd - The command to write.
 */
static inline void writeCommand(uint8 cmd)
{
	gpio.digitalWrite(activeDisplay->a0, LOW);
	gpio.spiDataRW(activeDisplay->channel, &cmd, 1);
} /* writeCommand */

/*
 * Write the data to the display driver.
 *
 * Parameters:
 *   data - The data to write.
 */
static inline void writeData(uint8 data)
{
	gpio.digitalWrite(activeDisplay->a0, HIGH);
	gpio.spiDataRW(activeDisplay->channel, &data, 1);
} /* writeData */

lcd_t *lcd_init(int spiSpeed, int channel, int cs, int a0, int rs)
{
	/* Create the one instance of the lcdst_t structure and activate it */
	lcd_t *instance = (lcd_t *) safeMalloc(sizeof(lcd_t));
	
	activeDisplay = instance;

	instance->channel = channel;
	instance->cs = cs;
	instance->a0 = a0;
	instance->rs = rs;
	/*
	 * instance->width; instance->height
	 * The setting of this variables will take place
	 * in the function lcdst_setOrientation() below.
	 */
	
	/* Configure the a0 pin. The logic level is not significant now. */
	gpio.pinMode(instance->a0, OUTPUT);
	
	/* If the rs pin is connected then configure it */
	if(instance->rs != -1)
	{
		gpio.pinMode(instance->rs, OUTPUT);
		gpio.digitalWrite(instance->rs, LOW);
		gpio.delay(10);
		gpio.digitalWrite(instance->rs, HIGH);
		gpio.delay(10);
	}
	
	/* Configure the SPI interface */
	if(gpio.spiSetup(instance->channel, spiSpeed) == -1)
	{
		fprintf(stderr, "Failed to setup the SPI interface!\n");
		exit(EXIT_FAILURE);
	}

	/* Software reset; Wait minimum 120ms */
	writeCommand(0x01);
	gpio.delay(150);
	
	/* Sleep out; Wait minimum 120ms */
	writeCommand(0x11);
	gpio.delay(150);
	
	/* Set the orientation and the gamma */
	lcd_setOrientation(instance, 0);
	lcd_setGamma(instance, 2); /* Optional */
	
	/* Set the pixel format */
	writeCommand(0x3A);
	writeData(0x06);
	
	/* Display ON; Wait 100ms before start */
	writeCommand(0x29);
	gpio.delay(100);
	
	return instance;
} /* lcd_init */

void lcd_deinit(lcd_t *display)
{
	if(display == NULL) return;
	
	free(display);
} /* lcdst_uninit */

void lcd_setOrientation(lcd_t* lcd, uint16 orientation)
{
	writeCommand(0x36); /* Memory Data Access Control */

	int sw = SCREEN_WIDTH;
	int sh = SCREEN_HEIGHT;

	uint8 my = 1 << 7; // row address order bit
	uint8 mx = 1 << 6; // column address order bit
	uint8 mv = 1 << 5; // row/column exchange bit
	switch(orientation)
	{
		case 1:
			writeData(mx & mv);
			activeDisplay->width  = sw;
			activeDisplay->height = sh;
			break;

		case 2:
			writeData(my & mx);
			activeDisplay->width  = sh;
			activeDisplay->height = sw;
			break;

		case 3:
			writeData(my & mv);
			activeDisplay->width  = sw;
			activeDisplay->height = sh;
			break;

		case 4:
			writeData(mx);
			activeDisplay->width  = sh;
			activeDisplay->height = sw;
			break;


		default:
			writeData(0); /* None */
			activeDisplay->width  = sh;
			activeDisplay->height = sw;
			break;
	}
	lcdhw_setWindow(lcd, 0, 0, activeDisplay->width - 1, activeDisplay->height - 1);
} /* lcdst_setOrientation */

void lcd_setGamma(lcd_t* lcd, uint16 state)
{
	/* The status (0 or 1) of the GS pin can only be empirically tested */
	switch(state)
	{
		case 1:  state = 2; break; /* GS_pin=1: 1.8; GS_pin=0: 2.5 */
		case 2:  state = 4; break; /* GS_pin=1: 2.5; GS_pin=0: 2.2 */
		case 3:  state = 8; break; /* GS_pin=1: 1.0; GS_pin=0: 1.8 */
		default: state = 1; break; /* GS_pin=1: 2.2; GS_pin=0: 1.0 */
	}
	
	/* Set built-in gamma */
	writeCommand(0x26);
	writeData(state);
} /* lcdst_setGamma */

void lcd_setInversion(lcd_t* lcd, uint16 state)
{
	/* Display inversion ON/OFF */
	writeCommand(state ? 0x21 : 0x20);
} /* lcdst_setInversion */

uint16 lcdhw_setWindow(lcd_t* lcd, uint16 x1, uint16 y1, uint16 x2, uint16 y2)
{
	/* Accept: 0 <= x1 <= x2 < activeDisplay->width */
	if(x2 < x1) return 1;
	if(x2 >= activeDisplay->width) return 1;
	
	/* Accept: 0 <= y1 <= y2 < activeDisplay->height */
	if(y2 < y1) return 1;
	if(y2 >= activeDisplay->height) return 1;
	
	/* Set column address */
	writeCommand(0x2A);
	writeData(x1 >> 8); writeData(x1 & 0xFF);
	writeData(x2 >> 8); writeData(x2 & 0XFF);
	
	/* Set row address */
	writeCommand(0x2B);
	writeData(y1 >> 8); writeData(y1 & 0xFF);
	writeData(y2 >> 8); writeData(y2 & 0xFF);
	
	/* Activate RAW write */
	writeCommand(0x2C);
	//gpio.delay(5);
	
	return 0;
} /* lcdst_setWindow */

void lcd_activateRamWrite(void)
{
	writeCommand(0x2C);
	//gpio.delay(5);
} /* lcdst_activateRamWrite */

uint8 pixel[3];

inline void lcdhw_pushPixel(lcd_t* lcd, uint8 r, uint8 g, uint8 b)
{
	gpio.digitalWrite(activeDisplay->a0, HIGH);
	pixel[0] = r;
	pixel[1] = g;
	pixel[2] = b;
	gpio.spiDataRW(activeDisplay->channel, pixel, 3);
}

void lcdhw_pushPixels(lcd_t* lcd, uint8* pixels, size_t count)
{
	gpio.digitalWrite(activeDisplay->a0, HIGH);
	gpio.spiDataRW(activeDisplay->channel, pixels, count * 3);
}

uint8 line_buffer[SCREEN_WIDTH*3]; // lcd->width or lcd->height

void lcd_redrawBuffer(lcd_t* lcd)
{
	for (int i = 0; i < lcd->height; i++) { 
		memcpy(line_buffer, &screen_buffer[i*lcd->width*3], lcd->width*3);
		lcdhw_setWindow(lcd, 0, i, lcd->width - 1, i);
		lcdhw_pushPixels(lcd, line_buffer, lcd->width);
	}
}

uint16 lcd_setWindow(lcd_t* lcd, uint16 x1, uint16 y1, uint16 x2, uint16 y2)
{
	screen_window_x1 = x1;
	screen_window_x2 = x2;
	screen_window_y1 = y1;
	screen_window_y2 = y2;
	screen_cursor_x = x1;
	screen_cursor_y = y1;
	return 0;
}

void lcd_pushPixel(lcd_t* lcd, uint8 r, uint8 g, uint8 b)
{
	int i = screen_cursor_x
		+ screen_cursor_y * lcd->width;
	screen_buffer[i * 3 + 0] = r;
	screen_buffer[i * 3 + 1] = g;
	screen_buffer[i * 3 + 2] = b;
	
	advance_screen_cursor();
}

void lcd_pushPixelSkip(lcd_t* lcd)
{
	advance_screen_cursor();
}

void lcd_pushPixels(lcd_t* lcd, uint8* pixels, size_t count)
{
	for (int i = 0; i < count; i++) {
		lcd_pushPixel(lcd,
			pixels[i * 3 + 0],
			pixels[i * 3 + 1],
			pixels[i * 3 + 2]);
	}
}