#include "sharpmem.h"
#include "fonts.h"

void SHARPMEM_TOGGLEVCOM(SharpMemDisplay_t *display) {
    display->_sharpmem_vcom = display->_sharpmem_vcom ? 0x00 : SHARPMEM_BIT_VCOM;  
}

void SHARPMEM_draw_pixel(SharpMemDisplay_t *display, uint16_t x, uint16_t y, bool black) {
    if ((x >= display->width) || (y >= display->height))
        return;

    if (black) {
        display->_buffer[(y * display->width + x) / 8] |= (0x1 << (x & 7)); //potentially expensive when run many times, use lookup from adafruit lib
    } else {
        display->_buffer[(y * display->width + x) / 8] &= ~(0x1 << (x & 7));
    }
}

void SHARPMEM_draw_line(SharpMemDisplay_t *display, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, bool black, uint8_t thickness) {

}

void SHARPMEM_draw_circle(SharpMemDisplay_t *display, uint16_t x, uint16_t y, uint8_t r, bool filled) {

}

void SHARPMEM_draw_rect(SharpMemDisplay_t *display, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, bool filled) {

}

void SHARPMEM_write(SharpMemDisplay_t *display, char *text, uint8_t font_index, uint16_t x0, uint16_t y0, bool inverse, bool force_bg) {
	/*uint8_t mask = 0xFF;
	uint8_t a1, a2, b;
	if(font_index > NUMBER_OF_FONTS) return;
	bitmap_font_t *font = fonts[font_index];

	for (uint8_t ci = 0; ci < strlen(text); ci++){
		uint16_t base_index = font->lookup[(uint8_t)text[ci]];
		uint8_t offset = (x0 + (ci)*font->char_w) % 8;

		for (uint8_t y = 0; y < font->char_h; y++) {
			if(y+y0 > display->height) return;

			uint8_t start_byte = (x0 + ci*font->char_w)/8 + (y0+y)*display->width/8;

			if((x0 + ci*font->char_w % 8))
				memcpy(display->_buffer + ((y+y0)*(display->width)/8) + x0/8 + ci*font->char_w/8 , font->font_bytes + base_index*font->bytes_per_char + y*font->bytes_per_row , font->bytes_per_row);
			else {
				uint8_t charbase = base_index*font->bytes_per_char + y*font->bytes_per_row;
				bool single_ended_last_byte = (offset + font->char_w - 1) / 8 > (bytes_per_row-1);
				uint8_t buffer_bytes = font->bytes_per_row + ( single_ended_last_byte ? 1 : 0 );
				for(uint8_t b_i = 0; b_i < buffer_bytes; b_i++) {
					b = display->_buffer[start_byte + b_i];
					a2 = font->font_bytes[ charbase + b_i ] >> offset;
					if(b_i == 0) {
						b = ((mask >> offset) & a2) + ( (mask << (8-offset)) & b );
					} else if (b_i != (buffer_bytes-1 + !single_ended_last_byte)) {
						a1 = font->font_bytes[ charbase + b_i - 1 ] << (8-offset);
						if (!single_ended_last_byte) {
							uint8_t remainder = (offset + font->char_w) % 8;
							b = ( (a1 + a2) & (mask << (8-remainder)) ) + ((mask >> remainder) & b);
						}
						else b = a1 + a2;
					} else {
						a1 = font->font_bytes[ charbase + b_i - 1 ] << (8-offset);
						uint8_t remainder = (offset + font->char_w) % 8;
						b = ((mask << (8-remainder)) & a1) + ( (mask >> remainder) & b );
					}
					display->_buffer[start_byte + b_i] = b;
				}
				start_byte += font->bytes_per_char;
				end_byte += font->bytes_per_char;
			}
		}
	}*/

	// potentially inefficient but simplest algorithm involving reading and writing every single pixel
	if(font_index > NUMBER_OF_FONTS) return;
	bitmap_font_t *font = fonts[font_index];

	for (uint8_t ci = 0; ci < strlen(text); ci++){
		uint16_t base_index = font->lookup[(uint8_t)text[ci]];

		for (uint16_t y = 0; y < font->char_h; y++) {
			uint16_t charbase = base_index*font->bytes_per_char + y*font->bytes_per_row;
			for (uint16_t x = 0; x < font->char_w; x++) {
				bool white = ( (font->font_bytes[charbase + x/8] & (0x1 << (x%8)) ) == 0 );
				if (!force_bg && white) continue;
				if (inverse) white = !white;
				SHARPMEM_draw_pixel(display, x0+ci*font->char_w+x, y0+y, !white);
			}
		}
	}
}

uint8_t SHARPMEM_get_pixel(SharpMemDisplay_t *display, uint16_t x, uint16_t y) {
	if ((x >= display->width) || (y >= display->height))
	   return 1; //1 = empty

}

void SHARPMEM_clear_display(SharpMemDisplay_t *display) {
	HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_RESET); // set lcdmode pin to low for manual vcom control

	memset(display->_buffer, 0xff, (display->width * display->height) / 8);

    // Send the clear screen command rather than doing a HW refresh (quicker)
    
    uint8_t clear_data[2] = {(uint8_t)(display->_sharpmem_vcom | SHARPMEM_BIT_CLEAR),
                            0x00};
    
    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_SET);
    HAL_SPI_Transmit(display->spidev, clear_data, 2, 16);
    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_RESET);

    SHARPMEM_TOGGLEVCOM(display);
    HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_SET); // set lcdmode pin to high for 1Hz external vcom signal
}

void SHARPMEM_refresh_display(SharpMemDisplay_t *display) {
	HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_RESET); // set lcdmode pin to low for manual vcom control
    uint16_t i, currentline;

    //spidev->beginTransaction();
    // Send the write command
    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_SET);
    uint8_t write_data[1] = {0x00 | SHARPMEM_BIT_WRITECMD};
    HAL_SPI_Transmit(display->spidev, write_data, 1, 100);
    SHARPMEM_TOGGLEVCOM(display);

    uint8_t bytes_per_line = display->width / 8;
    uint16_t totalbytes = (display->width * display->height) / 8;

    for (i = 0; i < totalbytes; i += bytes_per_line) {
        uint8_t line[bytes_per_line + 2];

        // Send address byte
        currentline = ((i + 1) / (display->width / 8)) + 1;
        line[0] = currentline;
        // copy over this line
        memcpy(line + 1, display->_buffer + i, bytes_per_line);
        // Send end of line
        line[bytes_per_line + 1] = 0x00;
        // send it!
        HAL_SPI_Transmit(display->spidev, line, bytes_per_line + 2, 16);
    }

    // Send another trailing 8 bits for the last line
    HAL_SPI_Transmit(display->spidev, (0x00), 1, 100);
    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_SET); // set lcdmode pin to high for 1Hz external vcom signal
}

void SHARPMEM_clear_display_buffer(SharpMemDisplay_t *display) {
	memset(display->_buffer, 0xff, (display->width * display->height) / 8);
}

uint8_t *SHARPMEM_get_buffer(SharpMemDisplay_t *display) {

}