#include <f4ll_cpp/ili9341.h>
#include <platform/core_ll.h>
#include <inttypes.h>
#include <string.h>
#include <stdarg.h>

#if defined(FREERTOS)
#include <FreeRTOS.h>
#include <task.h>
#endif

#define LCD_L 320
#define LCD_S 240

#define LCD_W LCD_L
#define LCD_H LCD_S

#define LCD_REG              (*((volatile unsigned short *) 0x60000000))     /* DC = 0 */
#define LCD_RAM              (*((volatile unsigned short *) 0x60080000))     /* DC = 1 */

#define RGB2U16(R,G,B) ((R & 0xf8) << 8 | (G & 0xfc) << 3 | (B & 0xf8) >> 3)
#define MSBSPLIT(x) ((uint8_t)(x >> 8)), ((uint8_t)x)

namespace f4ll_cpp {

Ili9341Fsmc::Ili9341Fsmc(volatile uint16_t *reg, volatile uint16_t *ram,
		DMA_TypeDef *dma, uint32_t dmaStream,
		bool horizontal)
: DmaHelper(dma, dmaStream)
, m_reg(reg ? reg : (volatile unsigned short *) 0x60000000)
, m_ram(ram ? ram : (volatile unsigned short *) 0x60080000)
, m_width(horizontal ? LCD_L : LCD_S)
, m_height(horizontal ? LCD_S : LCD_L)
, m_rectX(0)
, m_rectY(0)
, m_rectWidth(m_width)
, m_rectHeight(m_height)
{
	LL_DMA_EnableIT_TC(GetDma(), GetStream());
	LL_DMA_EnableIT_TE(GetDma(), GetStream());

	WriteCmd(ILI9341_RESET);
	vTaskDelay(pdMS_TO_TICKS(10));
	WriteCmd(ILI9341_DISPLAY_OFF);
	WriteCmd(ILI9341_POWERA, {0x39, 0x2C, 0x00, 0x34, 0x02});
	WriteCmd(ILI9341_POWERB, {0x00, 0xC1, 0x30});
	WriteCmd(ILI9341_DTCA, {0x85, 0x00, 0x78});
	WriteCmd(ILI9341_DTCB, {0x00, 0x00});
	WriteCmd(ILI9341_POWER_SEQ, {0x64, 0x03, 0x12, 0x81});
	WriteCmd(ILI9341_PRC, 0x20);
	WriteCmd(ILI9341_POWER1, 0x23);
	WriteCmd(ILI9341_POWER2, 0x10);
	WriteCmd(ILI9341_VCOM1, {0x3E, 0x28});
	WriteCmd(ILI9341_VCOM2, 0x86);
	WriteCmd(ILI9341_MAC, horizontal ? 0xe8 : 0x48);
	WriteCmd(ILI9341_PIXEL_FORMAT, 0x55);
	WriteCmd(ILI9341_FRC, {0x00, 0x18});
	WriteCmd(ILI9341_DFC, {0x08, 0x82, 0x27});
	WriteCmd(ILI9341_3GAMMA_EN, 0x00);
	WriteCmd(ILI9341_COLUMN_ADDR, {0x00, 0x00, (uint16_t)((m_width-1) >> 8), (uint16_t)((m_width-1) & 0x00ff)});
	WriteCmd(ILI9341_PAGE_ADDR, {0x00, 0x00, (uint16_t)((m_height-1) >> 8), (uint16_t)((m_height-1) & 0x00ff)});
	WriteCmd(ILI9341_GAMMA, 0x01);
	WriteCmd(ILI9341_PGAMMA, {0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E, 0xF1, 0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00});
	WriteCmd(ILI9341_NGAMMA, {0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31, 0xC1, 0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F});
	WriteCmd(ILI9341_SLEEP_OUT);
	vTaskDelay(pdMS_TO_TICKS(100));
	WriteCmd(ILI9341_DISPLAY_ON);
}

void Ili9341Fsmc::WriteCmd(uint16_t cmd, std::initializer_list<const uint16_t> params)
{
	*m_reg = cmd;
	for(uint16_t param : params)
		*m_ram = param;
}

void Ili9341Fsmc::WriteCmd_(uint16_t cmd, uint16_t cnt, ...)
{
	va_list argp;
	va_start(argp, cnt);

	*m_reg = cmd;
	for(uint16_t n = 0; n < cnt; ++n) {
		*m_ram = (uint16_t) va_arg(argp, uint32_t);
	}
	va_end(argp);
}

void Ili9341Fsmc::SetScrollMode(bool on)
{
	if(on != m_scrollMode) {
		if(on)
			WriteCmd(ILI9341_VERTICAL_SCROLLING_DEFINITION, {0x00, 0x00, MSBSPLIT(m_height), 0x00, 0x00});
		else
			WriteCmd(ILI9341_NORMAL_DISPLAY_MODE_ON);
		m_scrollMode = on;
	}
}

void Ili9341Fsmc::WritePixels(void *src, uint32_t count, bool increment, bool async)
{
	WriteCmd(ILI9341_GRAM);
	LL_DMA_SetM2MDstAddress(GetDma(), GetStream(), (uint32_t)m_ram);
	LL_DMA_SetMemoryIncMode(GetDma(), GetStream(), LL_DMA_MEMORY_NOINCREMENT);
	LL_DMA_SetM2MSrcAddress(GetDma(), GetStream(), (uint32_t) src);
	LL_DMA_SetPeriphIncMode(GetDma(), GetStream(), increment ? LL_DMA_PERIPH_NOINCREMENT : LL_DMA_PERIPH_NOINCREMENT);
	SetupDmaSize(count);
	m_dmaEngineBusy = true;
	LL_DMA_EnableStream(GetDma(), GetStream());
	if(!async)
		WaitDmaIdle();
}

void Ili9341Fsmc::ReadGRam(PixelPair *dst, uint32_t count, bool async)
{
	WriteCmd(ILI9341_MEMORY_READ);
	*m_ram;
	LL_DMA_SetM2MSrcAddress(GetDma(), GetStream(), reinterpret_cast<uint32_t>(m_ram));
	LL_DMA_SetPeriphIncMode(GetDma(), GetStream(), LL_DMA_PERIPH_NOINCREMENT);
	LL_DMA_SetM2MDstAddress(GetDma(), GetStream(), reinterpret_cast<uint32_t>(dst));
	LL_DMA_SetMemoryIncMode(GetDma(), GetStream(), LL_DMA_MEMORY_INCREMENT);
	SetupDmaSize(count);
	m_dmaEngineBusy = true;
	LL_DMA_EnableStream(GetDma(), GetStream());
	if(!async)
		WaitDmaIdle();
}

void Ili9341Fsmc::ReadPixels(uint16_t *dst, uint32_t count)
{
	uint16_t *barrier = dst + count;
	union {
		PixelPair	pp;
		uint16_t 	u16[3];
	} rawData;

	WriteCmd(ILI9341_MEMORY_READ);
	*m_ram;
	do {
		for(uint8_t n = 0; n < 3; ++n)
			rawData.u16[n] = *m_ram;
		*dst++ = RGB2U16(rawData.pp.r0, rawData.pp.g0, rawData.pp.b0);
		if(dst != barrier)
			*dst++ = RGB2U16(rawData.pp.r1, rawData.pp.g1, rawData.pp.b1);
	} while(dst != barrier);
}

void Ili9341Fsmc::SetScrollOffset()
{
	uint16_t offsetData[2] = { MSBSPLIT(m_scrollOffset) };
	WriteCmd(ILI9341_VERTICAL_SCROLLING_START_ADDRESS);
	*m_ram = offsetData[0];
	*m_ram = offsetData[1];
}

void Ili9341Fsmc::SetRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height)
{
	if(x > m_width || y > m_height || !width || !height )
		return;

	WaitDmaIdle();

	m_rectX = x;
	m_rectY = y;
	m_rectWidth = x + width > m_width ? m_width - x : width;
	m_rectHeight = y + height > m_height ? m_height - x : height;

	uint16_t right = x + m_rectWidth - 1;
	uint16_t bottom = y + m_rectHeight - 1;

	WriteCmd(ILI9341_COLUMN_ADDR, { MSBSPLIT(x), MSBSPLIT(right) });
	WriteCmd(ILI9341_PAGE_ADDR, { MSBSPLIT(y), MSBSPLIT(bottom) });
}

void Ili9341Fsmc::FillRect(uint16_t color, bool async)
{
	uint32_t count = m_rectWidth * m_rectHeight;
	WaitDmaIdle();
	m_dmaColor = color;
	WritePixels(&m_dmaColor, count, false, async);
}

void Ili9341Fsmc::FillRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color, bool async)
{
	SetRect(x, y, width, height);
	FillRect(color, async);
}

void Ili9341Fsmc::SetupDmaSize(uint32_t size)
{
	if(size > 0xffff) {
		LL_DMA_SetDataLength(GetDma(), GetStream(), 0xffff);
		m_dmaRemainingPixels = size - 0xffff;
	} else {
		LL_DMA_SetDataLength(GetDma(), GetStream(), size);
		m_dmaRemainingPixels = 0;
	}
}

void Ili9341Fsmc::HandleDmaIrq()
{
	if(*GetIsReg() & GetTcMask()) { //LL_DMA_IsActiveFlag_TCx(DMAy))
		*GetIfcReg() = GetTcMask();	//LL_DMA_ClearFlag_TCx(DMAy)
		LL_DMA_DisableStream(GetDma(), GetStream());
		if(!m_dmaRemainingPixels)
			m_dmaEngineBusy = false;
		else {
			SetupDmaSize(m_dmaRemainingPixels);
			LL_DMA_EnableStream(GetDma(), GetStream());
		}
	} else if(*GetIsReg() & GetTeMask()) //LL_DMA_IsActiveFlag_TEx(DMAy))
		*GetIfcReg() = GetTeMask();		 //	//LL_DMA_ClearFlag_TEx(DMAy)
}

void Ili9341Fsmc::SetCursor(uint16_t x, uint16_t y, uint16_t fgColor, uint16_t bgColor)
{
	m_xPos = x;
	m_yPos = y;
	m_fgColor = fgColor;
	m_bgColor = bgColor;
}

void Ili9341Fsmc::SetCursor(uint16_t x, uint16_t y)
{
	m_xPos = x;
	m_yPos = y;
}

void Ili9341Fsmc::PrintChar(char c, bool transparent)
{
	// fonts are rotated, one byte representing one column
	if(m_xPos > m_width - CHRWIDTH || m_yPos > m_height - CHRHEIGHT || c < ' ' || c >= '~')
		return;

	uint8_t const *chrPtr = m_font[c - 32];

	SetRect(m_xPos, AdjustY(m_yPos), CHRWIDTH, CHRHEIGHT);
	WaitDmaIdle();
	if(transparent)
		ReadPixels(m_fontBkgBuffer[0], CHRHEIGHT * CHRWIDTH);
	WriteCmd(ILI9341_GRAM);
	for(uint8_t y = 0; y < CHRHEIGHT; ++y ) {
		uint8_t mask = 1 << y;
		for(uint8_t x = 0; x < CHRWIDTH; ++x) {
			*m_ram = (chrPtr[x] & mask) ? m_fgColor : (transparent ? m_fontBkgBuffer[y][x] : m_bgColor);
		}
	}
	m_xPos += CHRWIDTH;
}

void Ili9341Fsmc::PrintChar(char c, uint16_t x, uint16_t y, uint16_t fgColor, uint16_t bgColor)
{
	SetCursor(x, y, fgColor, bgColor);
	PrintChar(c);
}

void Ili9341Fsmc::Print(char const *str, uint8_t len, bool transparent)
{
	if(!len)
		len = strlen(str);
	while(len--) {
		if(*str == '\r')
			m_xPos = 0;
		else if(*str == '\n') {
			m_yPos += CHRHEIGHT;
			if(m_yPos >= m_height) {
				m_yPos -= CHRHEIGHT;
				m_scrollOffset += CHRHEIGHT;
				if(m_scrollOffset >= m_height)
					m_scrollOffset -= m_height;
				WaitDmaIdle();
				SetScrollOffset();
				if(!transparent)
					FillRect(0, AdjustY(m_yPos), m_width, CHRHEIGHT, m_bgColor);
			}
		}
		else
			PrintChar(*str, transparent);
		++str;
	}
}

void Ili9341Fsmc::Delay(uint32_t ms)
{
#if defined(FREERTOS)
	vTaskDelay(pdMS_TO_TICKS(ms));
#else
	LL_mDelay(ms);
#endif
}


void Ili9341Fsmc::Test()
{
//	static PixelPair linebuf[320/2];
//	uint16_t x;

//	WriteCmd(ILI9341_READ_DISPLAY_PIXEL_FORMAT);
//	for( x= 0; x < 2; ++x)
//		reinterpret_cast<uint16_t*>(linebuf)[x] = *m_ram;


//	WriteCmd(ILI9341_GRAM);
//	uint16_t fillers[4] = { 0xf800, 0x7e0, 0x1f, 0xffff };
//	for(uint32_t y = 0; y < m_height; ++y) {
//		for(x = 0; x < m_width / 2; ++x)
//			*m_ram = fillers[ (x&0x18) >> 3 ];
//		for( ; x < m_width; ++x)
//			*m_ram = 0;
//	}
//
//	SetRect(0, 0, 11, 2);
//	ReadGRam(linebuf, 11 * 3);
}


const uint8_t Ili9341Fsmc::m_font[CHRCOUNT][CHRWIDTH] = {
	{0x00,0x00,0x00,0x00,0x00,0x00}, //
	{0x2f,0x00,0x00,0x00,0x00,0x00}, // !
	{0x03,0x00,0x03,0x00,0x00,0x00}, // "
	{0x12,0x3f,0x12,0x12,0x3f,0x12}, // #
	{0x2e,0x2a,0x7f,0x2a,0x3a,0x00}, // $
	{0x23,0x13,0x08,0x04,0x32,0x31}, // %
	{0x10,0x2a,0x25,0x2a,0x10,0x20}, // &
	{0x02,0x01,0x00,0x00,0x00,0x00}, // '
	{0x1e,0x21,0x00,0x00,0x00,0x00}, // (
	{0x21,0x1e,0x00,0x00,0x00,0x00}, // )
	{0x08,0x2a,0x1c,0x2a,0x08,0x08}, // *
	{0x08,0x08,0x3e,0x08,0x08,0x08}, // +
	{0x80,0x60,0x00,0x00,0x00,0x00}, // ,
	{0x08,0x08,0x08,0x08,0x08,0x00}, // -
	{0x30,0x30,0x00,0x00,0x00,0x00}, // .
	{0x20,0x10,0x08,0x04,0x02,0x00}, // /
	{0x1e,0x31,0x29,0x25,0x23,0x1e}, // 0
	{0x22,0x21,0x3f,0x20,0x20,0x20}, // 1
	{0x32,0x29,0x29,0x29,0x29,0x26}, // 2
	{0x12,0x21,0x21,0x25,0x25,0x1a}, // 3
	{0x18,0x14,0x12,0x3f,0x10,0x10}, // 4
	{0x17,0x25,0x25,0x25,0x25,0x19}, // 5
	{0x1e,0x25,0x25,0x25,0x25,0x18}, // 6
	{0x01,0x01,0x31,0x09,0x05,0x03}, // 7
	{0x1a,0x25,0x25,0x25,0x25,0x1a}, // 8
	{0x06,0x29,0x29,0x29,0x29,0x1e}, // 9
	{0x24,0x00,0x00,0x00,0x00,0x00}, // :
	{0x80,0x64,0x00,0x00,0x00,0x00}, // ;
	{0x08,0x14,0x22,0x00,0x00,0x00}, // <
	{0x14,0x14,0x14,0x14,0x14,0x00}, // =
	{0x22,0x14,0x08,0x00,0x00,0x00}, // >
	{0x02,0x01,0x01,0x29,0x05,0x02}, // ?
	{0x1e,0x21,0x2d,0x2b,0x2d,0x0e}, // @
	{0x3e,0x09,0x09,0x09,0x09,0x3e}, // A
	{0x3f,0x25,0x25,0x25,0x25,0x1a}, // B
	{0x1e,0x21,0x21,0x21,0x21,0x12}, // C
	{0x3f,0x21,0x21,0x21,0x12,0x0c}, // D
	{0x3f,0x25,0x25,0x25,0x25,0x21}, // E
	{0x3f,0x05,0x05,0x05,0x05,0x01}, // F
	{0x1e,0x21,0x21,0x21,0x29,0x1a}, // G
	{0x3f,0x04,0x04,0x04,0x04,0x3f}, // H
	{0x21,0x21,0x3f,0x21,0x21,0x21}, // I
	{0x10,0x20,0x20,0x20,0x20,0x1f}, // J
	{0x3f,0x04,0x0c,0x0a,0x11,0x20}, // K
	{0x3f,0x20,0x20,0x20,0x20,0x20}, // L
	{0x3f,0x02,0x04,0x04,0x02,0x3f}, // M
	{0x3f,0x02,0x04,0x08,0x10,0x3f}, // N
	{0x1e,0x21,0x21,0x21,0x21,0x1e}, // O
	{0x3f,0x09,0x09,0x09,0x09,0x06}, // P
	{0x1e,0x21,0x29,0x31,0x21,0x1e}, // Q
	{0x3f,0x09,0x09,0x09,0x19,0x26}, // R
	{0x12,0x25,0x25,0x25,0x25,0x18}, // S
	{0x01,0x01,0x01,0x3f,0x01,0x01}, // T
	{0x1f,0x20,0x20,0x20,0x20,0x1f}, // U
	{0x0f,0x10,0x20,0x20,0x10,0x0f}, // V
	{0x1f,0x20,0x10,0x10,0x20,0x1f}, // W
	{0x21,0x12,0x0c,0x0c,0x12,0x21}, // X
	{0x01,0x02,0x0c,0x38,0x04,0x02}, // Y
	{0x21,0x31,0x29,0x25,0x23,0x21}, // Z
	{0x3f,0x21,0x00,0x00,0x00,0x00}, // [
	{0x02,0x04,0x08,0x10,0x20,0x00}, // "\"
	{0x21,0x3f,0x00,0x00,0x00,0x00}, // ]
	{0x04,0x02,0x3f,0x02,0x04,0x00}, // ^
	{0x40,0x40,0x40,0x40,0x40,0x40}, // _
	{0x01,0x02,0x00,0x00,0x00,0x00}, // `
	{0x10,0x30,0x2a,0x2a,0x3c,0x00}, // a
	{0x3f,0x24,0x24,0x24,0x18,0x00}, // b
	{0x0c,0x14,0x22,0x22,0x00,0x00}, // c
	{0x18,0x24,0x24,0x24,0x3f,0x00}, // d
	{0x1c,0x2c,0x2a,0x2a,0x24,0x00}, // e
	{0x3e,0x05,0x01,0x00,0x00,0x00}, // f
	{0x18,0x28,0xa4,0xa4,0x7c,0x00}, // g
	{0x3f,0x04,0x04,0x0c,0x30,0x00}, // h
	{0x24,0x3d,0x20,0x00,0x00,0x00}, // i
	{0x20,0x40,0x40,0x3d,0x00,0x00}, // j
	{0x3f,0x0c,0x12,0x20,0x00,0x00}, // k
	{0x1f,0x20,0x20,0x00,0x00,0x00}, // l
	{0x3e,0x02,0x3c,0x02,0x3c,0x00}, // m
	{0x3e,0x02,0x02,0x02,0x3c,0x00}, // n
	{0x0c,0x14,0x22,0x32,0x0c,0x00}, // o
	{0xfc,0x24,0x24,0x24,0x18,0x00}, // p
	{0x18,0x24,0x24,0x24,0xfc,0x80}, // q
	{0x3c,0x04,0x02,0x02,0x00,0x00}, // r
	{0x24,0x2c,0x2a,0x2a,0x10,0x00}, // s
	{0x02,0x1f,0x22,0x20,0x00,0x00}, // t
	{0x1e,0x20,0x20,0x20,0x1e,0x00}, // u
	{0x06,0x18,0x20,0x18,0x06,0x00}, // v
	{0x1e,0x30,0x1c,0x30,0x0e,0x00}, // w
	{0x22,0x14,0x08,0x14,0x22,0x00}, // x
	{0x0c,0x10,0xa0,0xa0,0x7c,0x00}, // y
	{0x22,0x32,0x2a,0x26,0x22,0x22}, // z
	{0x0c,0x3f,0x21,0x00,0x00,0x00}, // {
	{0x3f,0x00,0x00,0x00,0x00,0x00}, // |
	{0x21,0x3f,0x0c,0x00,0x00,0x00}, // }
	{0x02,0x01,0x02,0x01,0x00,0x00}, // ~
	{0x00,0x00,0x00,0x00,0x00,0x00}
};

}	// f4ll_cpp