/*******************************************************************
 File: display.c
 Date: 30-September-2020
 Author: Peter Spindler
 Description: Display driver

 Based on display driver from STMicroelectronics: stm32f723e_discovery_lcd.c
 STM32Cube Firmware F7 Version 1.16.0, STM32F723E Discovery board

 License from STMicroelectronics:
  Redistribution and use in source and binary forms, with or without modification,
  are permitted provided that the following conditions are met:
    1. Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright notice,
       this list of conditions and the following disclaimer in the documentation
       and/or other materials provided with the distribution.
    3. Neither the name of STMicroelectronics nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
********************************************************************/

#include "stm32f7xx_hal.h"
#include "main.h"
#include "display.h"

#include <string.h>
#include <stdarg.h>
#include <stdio.h>

/* ToDo: Include headers for RTOS/mutex, declare mutex */

extern SRAM_HandleTypeDef hsram2;

#define DISPLAY_WIDTH    240
#define DISPLAY_HEIGHT   240

#define DISPLAY_ABS(X)  ((x) > 0 ? (x) : -(x))
#define DISPLAY_LIMIT( x, min, max ) {if( x < min ) x = min; else if( x > max ) x = max; }

static char Display_LineBuffer[64];  // Buffer for printf

// Display_Select: Called before any access to the display.
//   Return: 0: Error, do not access the display
//           1: OK, access the display
uint8_t Display_Select( void ) {
    /* ToDo: Take mutex */
	return 1; // OK
}

// Display_Deselect: Called after any access to the display.
void Display_Deselect( void ) {
    /* ToDo: Give mutex */
}

/*************************************************************************************************************
 Display: LowLevel
 **************************************************************************************************************/

typedef struct {
	__IO uint16_t REG;
	__IO uint16_t RAM;
} LCD_CONTROLLER_TypeDef;

#define FMC_BANK2_BASE  ((uint32_t)(0x60000000 | 0x04000000))
#define FMC_BANK2       ((LCD_CONTROLLER_TypeDef *) FMC_BANK2_BASE)

void Display_WriteCommand( uint8_t Reg) {
	FMC_BANK2->REG = Reg;
	__DSB();
}

void Display_WriteData( uint16_t Value ) {
	FMC_BANK2->RAM = Value;
	__DSB();
}

static uint16_t Display_ReadData(void) {
	return FMC_BANK2->RAM;
}

uint8_t Display_ReadReg(uint8_t Command) {
	Display_WriteCommand(Command);
	Display_ReadData();
	return (Display_ReadData());
}

// Command list format:
// Per command:
//   Byte 0: Command. 0xff: End of list
//   Byte 1: Number of arguments (N). Bit 7 set: delay, time in last argument
//   Byte 2,...,1+N: Arguments

static const uint8_t InitCmd[] = {
		0x10, 0x80,  10, // Sleep in, wait 10 ms
		0x01, 0x80, 200, // Reset, wait 100 ms
		0x11, 0x80, 120, // Sleep out, wait 120 ms
		0x36, 0,         // Normal Display
		0x3a, 1, 0x05,   // Color mode
		0x21, 0,         // Display inversion
		0x2a, 4, 0, 0, 0, 239, // Column address
		0x2b, 4, 0, 0, 0, 239, // Row address
		0xb2, 5, 0x0c, 0x0c, 0x00, 0x33, 0x33, // Porch
		0xb7, 1, 0x35, // Gate
		0xbb, 1, 0x1f, // Vcom
		0xc0, 1, 0x2c, // LCM
		0xc2, 2, 0x01, 0xc3, // VDV CRH EN
		0xc4, 1, 0x20, // VDV set
		0xc6, 1, 0x0f, // Frame rate control
		0xd0, 2, 0xa4, 0xa1, // Power control
		0xe0, 14, 0xd0, 0x08, 0x11, 0x08, 0x0c, 0x15, 0x39, 0x33, 0x50, 0x36, 0x13, 0x14, 0x29, 0x2d, // Positive Voltage Gamma Control
		0xe1, 14, 0xd0, 0x08, 0x10, 0x08, 0x06, 0x06, 0x39, 0x44, 0x51, 0x0b, 0x16, 0x14, 0x2f, 0x31, // Negative Voltage Gamma Control
		0x29, 0, // Display on
		0x11, 0, // Sleep out
		0x35, 1, 0x00, // Tearing Effect
		0xff
};

static void Display_WriteCommandList(const uint8_t *addr) {
	uint8_t  NumArgs;
	uint16_t Delay;

	while(*addr != 0xff) {
		//printf("Cmd %x, ", *addr);
		Display_WriteCommand(*addr++); // Command
		NumArgs  = *addr++;             // Number of arguments
		Delay    = NumArgs & 0x80;      // Bit 7: Delay flag
		NumArgs  &= ~0x80;
		//printf("Num: %d: ", NumArgs);
		while(NumArgs--) {
			//printf("%x ", *addr );
			Display_WriteData(*addr++);
		}
		//printf("\n");
		// Delay after command
		if(Delay) { // If delay flag set
			Delay = *addr++;            // Delay time
			if( Delay == 255) {
				Delay = 500;
			}
			//printf("Delay: %d\n", Delay );
			HAL_Delay(Delay);
		}
	}
}

static void Display_InitFmc( void ) {

	FMC_NORSRAM_TimingTypeDef  sram_timing={0};

	// PSRAM device configuration
	// Timing configuration derived from system clock (up to 216Mhz) for 108Mhz as PSRAM clock frequency
	sram_timing.AddressSetupTime      = 9;
	sram_timing.AddressHoldTime       = 2;
	sram_timing.DataSetupTime         = 6;
	sram_timing.BusTurnAroundDuration = 1;
	sram_timing.CLKDivision           = 2;
	sram_timing.DataLatency           = 2;
	sram_timing.AccessMode            = FMC_ACCESS_MODE_A;

	// Initialize the FMC controller for LCD (FMC_NORSRAM_BANK2)
	HAL_SRAM_Init(&hsram2, &sram_timing, &sram_timing);
}

static void Display_SetOrientation(uint32_t orientation) {
	uint8_t NormalDisplayParam;

	if( orientation == LCD_ORIENTATION_LANDSCAPE )  {
		NormalDisplayParam = 0x00;
	} else if( orientation == LCD_ORIENTATION_LANDSCAPE_ROT180 ) {
		// Vertical Scrolling Definition
		Display_WriteCommand(0x33);
		// TFA describes the Top Fixed Area
		Display_WriteData(0x00);
		Display_WriteData(0x00);
		// VSA describes the height of the Vertical Scrolling Area
		Display_WriteData(0x01);
		Display_WriteData(0xf0);
		// BFA describes the Bottom Fixed Area
		Display_WriteData(0x00);
		Display_WriteData(0x00);
		// Vertical Scroll Start Address of RAM:
		// GRAM row nbr (320) - Display row nbr (240) = 80 = 0x50
		Display_WriteCommand(0x37);
		Display_WriteData(0x00);
		Display_WriteData(0x50);

		NormalDisplayParam = 0xC0;
	} else  {
		NormalDisplayParam = 0x60;
	}
	Display_WriteCommand(0x36);
	Display_WriteData(NormalDisplayParam);
}

/*************************************************************************************************************
 Display: Primitives WITHOUT Select/Deselect
 **************************************************************************************************************/

static void Display_SetCursor_( uint16_t x, uint16_t y ) {
	DISPLAY_LIMIT( x, 0, DISPLAY_WIDTH-1 );
	DISPLAY_LIMIT( y, 0, DISPLAY_HEIGHT-1 );

	Display_WriteCommand( 0x2a );
	Display_WriteData( 0 );
	Display_WriteData( x );
	Display_WriteData( 0 );
	Display_WriteData( x+1 );

	Display_WriteCommand( 0x2b );
	Display_WriteData( 0 );
	Display_WriteData( y );
	Display_WriteData( 0 );
	Display_WriteData( y+1 );

	Display_WriteCommand( 0x2c );
}

static void Display_SetWindow_( uint16_t x, uint16_t y, uint16_t Width, uint16_t Height ) {

	DISPLAY_LIMIT( x, 0, DISPLAY_WIDTH-1 );
	DISPLAY_LIMIT( y, 0, DISPLAY_HEIGHT-1 );

	Display_WriteCommand( 0x2a );
	Display_WriteData( 0 );
	Display_WriteData( x );
	Display_WriteData( 0 );
	Display_WriteData( x+Width );

	Display_WriteCommand( 0x2b );
	Display_WriteData( 0 );
	Display_WriteData( y );
	Display_WriteData( 0 );
	Display_WriteData( y+Height );

	Display_WriteCommand( 0x2c );
}

static void Display_DrawHLine_( uint16_t x, uint16_t y, uint16_t Length, uint16_t Color ) {
	uint16_t counter = 0;
	Display_SetWindow_( x, y, DISPLAY_WIDTH-1, 0 );
	for(counter = 0; counter < Length; counter++) {
		Display_WriteData( Color );
	}
}

static void Display_DrawVLine_( uint16_t x, uint16_t y, uint16_t Length, uint16_t Color ) {
	uint16_t counter = 0;
	Display_SetWindow_( x, y, 0, DISPLAY_HEIGHT-1 );
	for(counter = 0; counter < Length; counter++) {
		Display_WriteData( Color );
	}
}

uint16_t Display_GetHeight( void ) {
	return DISPLAY_HEIGHT;
}

uint16_t Display_GetWidth( void ) {
	return DISPLAY_WIDTH;
}

static void Display_DrawPixel_( uint16_t x, uint16_t y, uint16_t Color ) {
	Display_SetCursor_( x, y );
	Display_WriteData( Color );
}

static void Display_DrawChar_( uint16_t x, uint16_t y, uint16_t ColorFront, uint16_t ColorBack, FONT_TypeDef *Font, char Char ) {
	uint32_t i = 0, j = 0;
	uint16_t Height, Width;
	uint8_t  Offset;
	const uint8_t *CharData;
	const uint8_t *LinePtr;
	uint32_t Line;

	Height = Font->Height;
	Width  = Font->Width;

	Offset =  8 *((Width + 7)/8) -  Width;

	CharData = &Font->table[(Char-' ')*Height * ((Width + 7) / 8)];

	Display_SetWindow_( x, y, Width-1, Height-1 );

	for( i = 0; i < Height; i++ ) {
		LinePtr = ((const uint8_t *)CharData + (Width + 7)/8 * i);

		switch(((Width + 7)/8))	{
		case 1: Line = LinePtr[0]; break;
		case 2: Line = (LinePtr[0]<< 8) | LinePtr[1]; break;
		case 3:
		default: Line = (LinePtr[0]<< 16) | (LinePtr[1]<< 8) | LinePtr[2]; break;
		}
		for( j = 0; j < Width; j++ ) {
			if( Line & (1 << (Width- j + Offset- 1))) {
				Display_WriteData( ColorFront );
			} else {
				Display_WriteData( ColorBack );
			}
		}
		y++;
	}
}

static void Display_PrintString_( uint16_t x, uint16_t y, uint16_t ColorFront, uint16_t ColorBack, FONT_TypeDef *Font, char *String ) {
	DISPLAY_ALIGN_TypeDef Mode = ALIGN_LEFT;

	if( Mode != ALIGN_LEFT ) {
		size_t StrLen = strlen(String);
		if( Mode == ALIGN_CENTER ) {
			x -= StrLen*Font->Width/2;
		} else if( Mode == ALIGN_RIGHT ) {
			x -= StrLen*Font->Width;
		}
		if( x < 0 ) {
			x = 0;
		}
	}
	while( *String ) {
		if( x >= Display_GetWidth() ) {
			break;
		}
		Display_DrawChar_( x, y, ColorFront, ColorBack, Font, *String );
		x += Font->Width;
		String ++;
	}
}

static void Display_DrawLine_( uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t Color ) {
	int16_t deltax = 0, deltay = 0, x = 0, y = 0, xinc1 = 0, xinc2 = 0,
			yinc1 = 0, yinc2 = 0, den = 0, num = 0, numadd = 0, numpixels = 0,
			curpixel = 0;

	deltax = DISPLAY_ABS(x2 - x1);
	deltay = DISPLAY_ABS(y2 - y1);
	x = x1;
	y = y1;

	if (x2 >= x1) {
		xinc1 = 1;
		xinc2 = 1;
	} else {
		xinc1 = -1;
		xinc2 = -1;
	}

	if( y2 >= y1 ) {
		yinc1 = 1;
		yinc2 = 1;
	} else {
		yinc1 = -1;
		yinc2 = -1;
	}

	if( deltax >= deltay ) {        // There is at least one x-value for every y-value
		xinc1 = 0;                  // Don't change the x when numerator >= denominator
		yinc2 = 0;                  // Don't change the y for every iteration
		den = deltax;
		num = deltax / 2;
		numadd = deltay;
		numpixels = deltax;         // There are more x-values than y-values
	} else {                        // There is at least one y-value for every x-value
		xinc2 = 0;                  // Don't change the x for every iteration
		yinc1 = 0;                  // Don't change the y when numerator >= denominator
		den = deltay;
		num = deltay / 2;
		numadd = deltax;
		numpixels = deltay;         // There are more y-values than x-values
	}

	for( curpixel = 0; curpixel <= numpixels; curpixel++ ) {
		Display_DrawPixel_( x, y, Color );
		num += numadd;                            // Increase the numerator by the top of the fraction
		if (num >= den) {                         // Check if numerator >= denominator
			num -= den;                             // Calculate the new numerator value
			x += xinc1;                             // Change the x as appropriate
			y += yinc1;                             // Change the y as appropriate
		}
		x += xinc2;                               // Change the x as appropriate
		y += yinc2;                               // Change the y as appropriate
	}
}

static void Display_Clear_( uint16_t Color ) {
	uint16_t y = 0;
	uint16_t Height = 0;
	uint16_t Width = Display_GetWidth();

	Height = Display_GetHeight();
	for( y = 0; y < Height ; y ++ ) {
		Display_DrawHLine_( 0, y, Width, Color );
	}
}

static void Display_FillTriangle_( uint16_t x1, uint16_t x2, uint16_t x3, uint16_t y1, uint16_t y2, uint16_t y3, uint16_t Color ) {
	int16_t deltax = 0, deltay = 0, x = 0, y = 0, xinc1 = 0, xinc2 = 0,
			yinc1 = 0, yinc2 = 0, den = 0, num = 0, numadd = 0, numpixels = 0,
			curpixel = 0;

	deltax = DISPLAY_ABS(x2 - x1);
	deltay = DISPLAY_ABS(y2 - y1);
	x = x1;
	y = y1;

	if (x2 >= x1) {
		xinc1 = 1;
		xinc2 = 1;
	} else {
		xinc1 = -1;
		xinc2 = -1;
	}

	if (y2 >= y1) {
		yinc1 = 1;
		yinc2 = 1;
	} else {
		yinc1 = -1;
		yinc2 = -1;
	}

	if (deltax >= deltay) {        // There is at least one x-value for every y-value
		xinc1 = 0;                  // Don't change the x when numerator >= denominator
		yinc2 = 0;                  // Don't change the y for every iteration
		den = deltax;
		num = deltax / 2;
		numadd = deltay;
		numpixels = deltax;         // There are more x-values than y-values
	} else {                          // There is at least one y-value for every x-value
		xinc2 = 0;                  // Don't change the x for every iteration
		yinc1 = 0;                  // Don't change the y when numerator >= denominator
		den = deltay;
		num = deltay / 2;
		numadd = deltax;
		numpixels = deltay;         // There are more y-values than x-values
	}

	for( curpixel = 0; curpixel <= numpixels; curpixel++ ) {
		Display_DrawLine_( x, y, x3, y3, Color );

		num += numadd;              // Increase the numerator by the top of the fraction
		if( num >= den ) {          // Check if numerator >= denominator
			num -= den;               // Calculate the new numerator value
			x += xinc1;               // Change the x as appropriate
			y += yinc1;               // Change the y as appropriate
		}
		x += xinc2;                 // Change the x as appropriate
		y += yinc2;                 // Change the y as appropriate
	}
}


/*************************************************************************************************************
 Display: High level with Select/Deselect
 **************************************************************************************************************/

void Display_On( void ) {
	if( !Display_Select() ) return;
	Display_WriteCommand( 0x29 );
	Display_WriteCommand( 0x11 );
	Display_Deselect();
}

void Display_Off( void ) {
	if( !Display_Select() ) return;
	Display_WriteCommand( 0xbd );
	Display_WriteData( 0xfe );
	Display_WriteCommand( 0x10 );
	Display_Deselect();
}

void Display_Clear( uint16_t Color ) {
	if( !Display_Select() ) return;
	Display_Clear_( Color );
	Display_Deselect();

}

void Display_DrawPixel( uint16_t x, uint16_t y, uint16_t Color ) {
	if( !Display_Select() ) return;
	Display_DrawPixel_( x, y, Color );
	Display_Deselect();
}

uint16_t Display_ReadPixel( uint16_t x, uint16_t y ) {
	uint16_t Color;
	if( !Display_Select() ) return 0;
	Display_SetCursor_( x, y );
	Display_WriteCommand( 0x2e );
	Color = Display_ReadData();
	Display_Deselect();
	return Color;
}

uint16_t Display_GetStringWidth( FONT_TypeDef *Font, char *String ) {
	return (uint16_t) strlen(String)*Font->Width;
}

uint16_t Display_GetFontCharHeight( FONT_TypeDef *Font ) {
	return Font->Height;
}

void Display_PrintString( uint16_t x, uint16_t y, uint16_t ColorFront, uint16_t ColorBack, FONT_TypeDef *Font, char *String ) {
	if( !Display_Select() ) return;
	Display_PrintString_( x, y, ColorFront, ColorBack, Font, String );
	Display_Deselect();
}

void Display_Printf( uint16_t x, uint16_t y, uint16_t ColorFront, uint16_t ColorBack, FONT_TypeDef *Font, char *Format, ... ) {
	va_list Args;
	va_start( Args, Format );

	if( !Display_Select() ) return;
	vsnprintf( Display_LineBuffer, sizeof(Display_LineBuffer), Format, Args );
	va_end( Args );
	Display_PrintString_( x, y, ColorFront, ColorBack, Font, Display_LineBuffer );

	Display_Deselect();
}

void Display_DrawLine( uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, uint16_t Color ) {
	if( !Display_Select() ) return;
	Display_DrawLine_( x1, y1, x2, y2, Color );
	Display_Deselect();
}

void Display_DrawRect( uint16_t x, uint16_t y, uint16_t Width, uint16_t Height, uint16_t Color ) {
	if( !Display_Select() ) return;
	Display_DrawHLine_( x, y, Width, Color );
	Display_DrawHLine_( x, (y+ Height), Width, Color );
	Display_DrawVLine_( x, y, Height, Color );
	Display_DrawVLine_( (x + Width), y, Height, Color );
	Display_Deselect();
}

void Display_FillRect( uint16_t x, uint16_t y, uint16_t Width, uint16_t Height, uint16_t Color ) {
	uint16_t i;
	if( !Display_Select() ) return;
	for( i = 0; i < Height ; i ++ ) {
		Display_DrawHLine_( x, y+i, Width, Color );
	}
	Display_Deselect();
}

void Display_DrawCircle( uint16_t x, uint16_t y, uint16_t Radius, uint16_t Color ) {
	int32_t   decision;
	uint32_t  current_x;
	uint32_t  current_y;

	if( !Display_Select() ) return;

	decision = 3 - (Radius << 1);
	current_x = 0;
	current_y = Radius;

	while (current_x <= current_y) {
		Display_DrawPixel_((x + current_x), (y - current_y), Color);
		Display_DrawPixel_((x - current_x), (y - current_y), Color);
		Display_DrawPixel_((x + current_y), (y - current_x), Color);
		Display_DrawPixel_((x - current_y), (y - current_x), Color);
		Display_DrawPixel_((x + current_x), (y + current_y), Color);
		Display_DrawPixel_((x - current_x), (y + current_y), Color);
		Display_DrawPixel_((x + current_y), (y + current_x), Color);
		Display_DrawPixel_((x - current_y), (y + current_x), Color);

		if (decision < 0) {
			decision += (current_x << 2) + 6;
		} else {
			decision += ((current_x - current_y) << 2) + 10;
			current_y--;
		}
		current_x++;
	}

	Display_Deselect();
}

void Display_FillCircle( uint16_t x, uint16_t y, uint16_t Radius, uint16_t Color ) {
	int32_t  decision;
	uint32_t  current_x;
	uint32_t  current_y;

	decision = 3 - (Radius << 1);

	current_x = 0;
	current_y = Radius;

	if( !Display_Select() ) return;

	while (current_x <= current_y) {
		if(current_y > 0) {
			Display_DrawHLine_( x - current_y, y + current_x, 2*current_y, Color );
			Display_DrawHLine_( x - current_y, y - current_x, 2*current_y, Color );
		}

		if(current_x > 0) {
			Display_DrawHLine_( x - current_x, y - current_y, 2*current_x, Color );
			Display_DrawHLine_( x - current_x, y + current_y, 2*current_x, Color );
		}

		if (decision < 0) {
			decision += (current_x << 2) + 6;
		} else {
			decision += ((current_x - current_y) << 2) + 10;
			current_y--;
		}
		current_x++;
	}
	Display_Deselect();

	Display_DrawCircle( x, y, Radius, Color );
}

void Display_DrawEllipse( uint16_t x, uint16_t y, uint16_t xRadius, uint16_t yRadius, uint16_t Color ) {
	int16_t xCur = 0, yCur = -yRadius, err = 2-2*xRadius, e2;
	float k = 0, rad1 = 0, rad2 = 0;

	rad1 = xRadius;
	rad2 = yRadius;

	k = (float)(rad2/rad1);

	if( !Display_Select() ) return;

	while (yCur <= 0) {
		Display_DrawPixel_((x-(uint16_t)(xCur/k)), (y+yCur), Color);
		Display_DrawPixel_((x+(uint16_t)(xCur/k)), (y+yCur), Color);
		Display_DrawPixel_((x+(uint16_t)(xCur/k)), (y-yCur), Color);
		Display_DrawPixel_((x-(uint16_t)(xCur/k)), (y-yCur), Color);

		e2 = err;
		if (e2 <= xCur) {
			err += ++xCur*2+1;
			if (-yCur == xCur && e2 <= yCur) e2 = 0;
		}
		if (e2 > yCur) err += ++yCur*2+1;
	}

	Display_Deselect();
}

void Display_FillEllipse( uint16_t x, uint16_t y, uint16_t xRadius, uint16_t yRadius, uint16_t Color ) {
	int16_t xCur = 0, yCur = -yRadius, err = 2-2*xRadius, e2;
	float k = 0, rad1 = 0, rad2 = 0;

	rad1 = xRadius;
	rad2 = yRadius;

	k = (float)(rad2/rad1);

	if( !Display_Select() ) return;

	while (yCur <= 0) {
		Display_DrawHLine_((x-(uint16_t)(xCur/k)), (y+yCur), (2*(uint16_t)(xCur/k) + 1), Color );
		Display_DrawHLine_((x-(uint16_t)(xCur/k)), (y-yCur), (2*(uint16_t)(xCur/k) + 1), Color );

		e2 = err;
		if (e2 <= xCur)
		{
			err += ++xCur*2+1;
			if (-yCur == xCur && e2 <= yCur) e2 = 0;
		}
		if (e2 > yCur) err += ++yCur*2+1;
	}

	Display_Deselect();
}


void Display_DrawPolygon( DISPLAY_POINT_TypeDef *Points, uint16_t PointCount, uint16_t Color ) {
	int16_t x = 0, y = 0;
	if(PointCount < 2)  {
		return;
	}

	if( !Display_Select() ) return;

	Display_DrawLine_( Points->x, Points->y, (Points+PointCount-1)->x, (Points+PointCount-1)->y, Color );
	while(--PointCount) {
		x = Points->x;
		y = Points->y;
		Points++;
		Display_DrawLine_( x, y, Points->x, Points->y, Color );
	}

	Display_Deselect();
}

void Display_FillTriangle( uint16_t x1, uint16_t x2, uint16_t x3, uint16_t y1, uint16_t y2, uint16_t y3, uint16_t Color ) {
	if( !Display_Select() ) return;
	Display_FillTriangle_( x1, x2, x3, y1, y2, y3, Color );
	Display_Deselect();
}


void Display_FillPolygon( DISPLAY_POINT_TypeDef *Points, uint16_t PointCount, uint16_t Color ) {
	int16_t X = 0, Y = 0, X2 = 0, Y2 = 0, X_center = 0, Y_center = 0, X_first = 0, Y_first = 0, pixelX = 0, pixelY = 0, counter = 0;
	uint16_t  IMAGE_LEFT = 0, IMAGE_RIGHT = 0, IMAGE_TOP = 0, IMAGE_BOTTOM = 0;

	IMAGE_LEFT = IMAGE_RIGHT = Points->x;
	IMAGE_TOP= IMAGE_BOTTOM = Points->y;

	for(counter = 1; counter < PointCount; counter++) {
		pixelX = Points[counter].x;
		if(pixelX < IMAGE_LEFT) {
			IMAGE_LEFT = pixelX;
		}
		if(pixelX > IMAGE_RIGHT) {
			IMAGE_RIGHT = pixelX;
		}

		pixelY = Points[counter].y;
		if(pixelY < IMAGE_TOP) {
			IMAGE_TOP = pixelY;
		}
		if(pixelY > IMAGE_BOTTOM) {
			IMAGE_BOTTOM = pixelY;
		}
	}

	if(PointCount < 2) {
		return;
	}

	X_center = (IMAGE_LEFT + IMAGE_RIGHT)/2;
	Y_center = (IMAGE_BOTTOM + IMAGE_TOP)/2;

	X_first = Points->x;
	Y_first = Points->y;

	if( !Display_Select() ) return;

	while( --PointCount ) {
		X = Points->x;
		Y = Points->y;
		Points++;
		X2 = Points->x;
		Y2 = Points->y;

		Display_FillTriangle_( X, X2, X_center, Y, Y2, Y_center, Color );
		Display_FillTriangle_( X, X_center, X2, Y, Y_center, Y2, Color );
		Display_FillTriangle_( X_center, X2, X, Y_center, Y2, Y, Color);
	}

	Display_FillTriangle_( X_first, X2, X_center, Y_first, Y2, Y_center, Color );
	Display_FillTriangle_( X_first, X_center, X2, Y_first, Y_center, Y2, Color );
	Display_FillTriangle_( X_center, X2, X_first, Y_center, Y2, Y_first, Color );

	Display_Deselect();
}


void Display_Test( void ) {
	Display_Clear( LCD_COLOR_BLACK );
	Display_PrintString(0,0,LCD_COLOR_WHITE,LCD_COLOR_BLACK,&FontBig,"Hallo");
	Display_DrawLine( 0, 0, Display_GetWidth()-1, Display_GetHeight()-1, LCD_COLOR_BLUE );
	Display_DrawLine( 0, Display_GetHeight()-1, Display_GetWidth()-1, 0, LCD_COLOR_BLUE );
	Display_DrawLine( 0, 0, Display_GetWidth()-1, 0, LCD_COLOR_GREEN );
	Display_DrawLine( 0, 0, 0, Display_GetHeight()-1, LCD_COLOR_GREEN );
	Display_DrawLine( 0, 10, Display_GetWidth()-1, 20, LCD_COLOR_RED );

	Display_DrawRect( 50, 50, 40, 20, LCD_COLOR_YELLOW );
	Display_FillRect( 150, 50, 40, 20, LCD_COLOR_YELLOW );

	Display_DrawCircle( 80, 120, 30, LCD_COLOR_MAGENTA );
	Display_FillCircle( 180, 120, 30, LCD_COLOR_MAGENTA );

	Display_DrawEllipse( 80, 180, 20, 30, LCD_COLOR_CYAN );
	Display_FillEllipse( 180, 180, 30, 20, LCD_COLOR_CYAN );

	DISPLAY_POINT_TypeDef Points[4] = {	{20,20}, {50,10}, {40,40}, {30,50} };
	Display_DrawPolygon( Points, 4, LCD_COLOR_LIGHTBLUE );

	uint8_t i;
	for( i=0; i<4; i++ ) {
		Points[i].x += 100;
	}
	Display_FillPolygon( Points, 4, LCD_COLOR_LIGHTBLUE );
}

void Display_Init( void ) {

	// Backlight control signal assertion:
	HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET);

	// Apply hardware reset according to procedure indicated in FRD154BP2901 documentation:
	HAL_GPIO_WritePin(LCD_RST_GPIO_Port, LCD_RST_Pin, GPIO_PIN_RESET);
	HAL_Delay(5);
	HAL_GPIO_WritePin(LCD_RST_GPIO_Port, LCD_RST_Pin, GPIO_PIN_SET);
	HAL_Delay(10);
	HAL_GPIO_WritePin(LCD_RST_GPIO_Port, LCD_RST_Pin, GPIO_PIN_RESET);
	HAL_Delay(20);
	HAL_GPIO_WritePin(LCD_RST_GPIO_Port, LCD_RST_Pin, GPIO_PIN_SET);
	HAL_Delay(10);

	Display_InitFmc();

	uint8_t DisplayId = Display_ReadReg(0x04);

	if( DisplayId == 0x85 ) {
		Display_WriteCommandList( InitCmd );
		Display_SetOrientation( LCD_ORIENTATION_LANDSCAPE_ROT180 );
		Display_Clear_( LCD_COLOR_BLACK ); // Use variant without select/deselect to ignore OS and Mutex here!
	}

	/* ToDo: Create Mutex */
}