#include "VGA.h"
#include "GPIO.h"
#include "RCC.h"

#include <stdlib.h>

volatile uint32_t VGALine;
volatile uint32_t VGAFrame;
static uint32_t VGAFrameBufferAddress;
static uint32_t VGACurrentLineAddress;
static uint32_t VGAPixelsPerRow;
static uint16_t VGALineMultiplierCounter;

static HBlankInterruptFunction *HBlankInterruptHandler;

void InitializeVGA()
{
	#ifdef EnableOverclocking
	InitializeVGAWithTimings(
	2796,   // 88 MHz / 31.46875 kHz = 2796.42502483
	336,    // 88 MHz * 3.813 microseconds = 335.544 - sync pulse end
	503-14);   // 88 MHz * (3.813 + 1.907) microseconds = 503.36 - back porch end, start pixel clock
	#else
	InitializeVGAWithTimings(
	2669, // 84 MHz / 31.46875 kHz = 2669.31479643
	320,  // 84 MHz * 3.813 microseconds = 320.292 - sync pulse end
	480); // 84 MHz * (3.813 + 1.907) microseconds = 480.48 - back porch end, start pixel clock
	#endif
}

void InitializeVGAFor800x600()
{
	#ifdef EnableOverclocking
	InitializeVGAWithTimings(
	2323,    // 88 MHz / 37.878787 kHz = 2323.2000539
	281,     // 88 MHz * 3.2 microseconds = 281.6 - sync pulse end
	457-14); // 88 MHz * (3.2 + 2.2) microseconds = 457.2 - back porch end, start pixel clock
	#else
	InitializeVGAWithTimings(
	2218,    // 84 MHz / 37.878787 kHz = 2217.60005145
	269,     // 84 MHz * 3.2 microseconds = 268.8 - sync pulse end
	454-14); // 84 MHz * (3.2 + 2.2) microseconds = 453.6 - back porch end, start pixel clock
	#endif
}

void InitializeVGAFor1024x768()
{
	#ifdef EnableOverclocking
	InitializeVGAWithTimings(
	1820,    // 88 MHz / 48.363095238095 kHz = 1 819.56923
	184,     // 88 MHz * 2.0923076923077 microseconds = 184.123077 - sync pulse end
	401-14); // 88 MHz * (2.0923076923077 + 2.4615384615385) microseconds = 400.738462 - back porch end, start pixel clock
	#else
	InitializeVGAWithTimings(
	1736,    // 84 MHz / 48.363095238095 kHz = 1 736.86154
	176,     // 84 MHz * 2.0923076923077 microseconds = 175.753846 - sync pulse end
	383-14); // 84 MHz * (2.0923076923077 + 2.4615384615385) microseconds = 382.523077 - back porch end, start pixel clock
	#endif
}
 
void InitializeVGAWithTimings(uint32_t hsynccycles,uint32_t pulselength,uint32_t pulseandbackporchlength)
{
	VGALine=-1;
	VGAFrame=0;

	HBlankInterruptHandler=NULL;

	// Turn on peripherals.
	EnableAHB1PeripheralClock(RCC_AHB1ENR_GPIOBEN|RCC_AHB1ENR_GPIOEEN|RCC_AHB1ENR_DMA2EN);
	EnableAPB2PeripheralClock(RCC_APB2ENR_TIM8EN|RCC_APB2ENR_SYSCFGEN);
	EnableAPB1PeripheralClock(RCC_APB1ENR_TIM2EN);

	// Configure DAC pins, and set to black.
	SetGPIOOutputMode(GPIOE,0xff00);
	SetGPIOPushPullOutput(GPIOE,0xff00);
	SetGPIOSpeed50MHz(GPIOE,0xff00);
	SetGPIOPullUpResistor(GPIOE,0xff00);
	GPIOE->BSRRH=0xff00;

	// Configure sync pins and drive them high.
	// Also link HSync-pin (PB11) to TIM2 CC-channel 4.
	SetGPIOAlternateFunctionMode(GPIOB,1<<11);
	SelectAlternateFunctionForGPIOPin(GPIOB,11,1); // TIM2_CH4
	SetGPIOOutputMode(GPIOB,1<<12);
	SetGPIOPushPullOutput(GPIOB,(1<<11)|(1<<12));
	SetGPIOSpeed50MHz(GPIOB,(1<<11)|(1<<12));
	SetGPIOPullUpResistor(GPIOB,(1<<11)|(1<<12));
	GPIOB->BSRRL=(1<<11)|(1<<12);


	// Configure timer 2 as the HSync timer. Timer 2 runs at half frequency, thus 84 MHz.
	// CC4 is used to generate the HSync pulse, using PWM mode and driving the pin directly.
	// CC3 is used to generate a trigger signal for TIM8, which drives the pixel DMA.
	TIM2->CR1=TIM_CR1_ARPE;
	TIM2->CR2=(6*TIM_CR2_MMS_0); // Trigger-out on CCR3.
	TIM2->DIER=0; // No interrupts enable. They will be enabled later.
	TIM2->CCER=0; // Disable CC, so we can program it.
	TIM2->CCMR1=0;
	// PWM-mode: Channel 4 set to active level on reload, passive level after CC4-match.
	// Channel 3 set to passive level on reload, active level after CC3-match.
	TIM2->CCMR2=(6*TIM_CCMR2_OC4M_0)|(7*TIM_CCMR2_OC3M_0);
	TIM2->CCER=TIM_CCER_CC4E|TIM_CCER_CC4P; // Channel 4 enabled, reversed polarity (active low).
	TIM2->PSC=0; // Prescaler = 1

	TIM2->ARR=hsynccycles-1;
	// On CNT==0: Sync pulse start.
	TIM2->CCR4=pulselength; // Sync pulse end.
	TIM2->CCR3=pulseandbackporchlength; // Back porch end, start pixel clock.

	// Enable HSync timer.
	TIM2->CNT=-10; // Make sure it hits ARR. 
	TIM2->CR1|=TIM_CR1_CEN;
}

static inline void WaitForLastLineIfVGAEnabled()
{
	if(!IsInterruptEnabled(TIM2_IRQn)) return;
	while(VGALine!=0xffffffff);
}

static void SetVGAHorizontalSyncAtInterrupt(InterruptHandler *handler,int dier)
{
	WaitForLastLineIfVGAEnabled();

	TIM2->DIER=dier;
	TIM2->CCER=TIM_CCER_CC4E|TIM_CCER_CC4P; // Channel 4 enabled, reversed polarity (active low).

	// Enable HSync timer interrupt and set highest priority.
	InstallInterruptHandler(TIM2_IRQn,handler);
	EnableInterrupt(TIM2_IRQn);
	SetInterruptPriority(TIM2_IRQn,0);
}

static void SetVGAHorizontalSyncActiveHighAtInterrupt(InterruptHandler *handler,int dier)
{
	WaitForLastLineIfVGAEnabled();

	TIM2->DIER=dier; // Enable update interrupt.
	TIM2->CCER=TIM_CCER_CC4E; // Channel 4 enabled, normal polarity (active high).

	// Enable HSync timer interrupt and set highest priority.
	InstallInterruptHandler(TIM2_IRQn,handler);
	EnableInterrupt(TIM2_IRQn);
	SetInterruptPriority(TIM2_IRQn,0);
}

void SetVGAHorizontalSyncForDMA(InterruptHandler *handler)
{
	SetVGAHorizontalSyncAtInterrupt(handler,TIM_DIER_UIE); // Use update interrupt.
}

void SetVGAHorizontalSyncActiveHighForDMA(InterruptHandler *handler)
{
	SetVGAHorizontalSyncActiveHighAtInterrupt(handler,TIM_DIER_UIE); // Use update interrupt.
}

void SetVGAHorizontalSync(InterruptHandler *handler)
{
	TIM2->CCR2=TIM2->CCR3;
	SetVGAHorizontalSyncAtInterrupt(handler,TIM_DIER_CC2IE); // Use CC2 interrupt.
}

void SetVGAHorizontalSyncActiveHigh(InterruptHandler *handler)
{
	TIM2->CCR2=TIM2->CCR3;
	SetVGAHorizontalSyncActiveHighAtInterrupt(handler,TIM_DIER_CC2IE); // Use CC2 interrupt.
}

void SetVGAHorizontalSyncWithEarlyStart(InterruptHandler *handler,int offset)
{
	TIM2->CCR2=TIM2->CCR3-offset;
	SetVGAHorizontalSyncAtInterrupt(handler,TIM_DIER_CC2IE); // Use CC2 interrupt.
}

void SetVGAHorizontalSyncActiveHighWithEarlyStart(InterruptHandler *handler,int offset)
{
	TIM2->CCR2=TIM2->CCR3-offset;
	SetVGAHorizontalSyncActiveHighAtInterrupt(handler,TIM_DIER_CC2IE); // Use CC2 interrupt.
}

void SetHBlankInterruptHandler(HBlankInterruptFunction *handler)
{
	HBlankInterruptHandler=handler;
}




static void BlankHSyncHandler480();
static void BlankHSyncHandler400();
static void BlankHSyncHandler350();
static void BlankHSyncHandler600();
static void BlankHSyncHandler768();

void SetBlankVGAScreenMode480()
{
	SetVGASignalToBlack();
	SetVGAHorizontalSyncForDMA(BlankHSyncHandler480);
}

void SetBlankVGAScreenMode400()
{
	SetVGASignalToBlack();
	SetVGAHorizontalSyncForDMA(BlankHSyncHandler400);
}

void SetBlankVGAScreenMode350()
{
	SetVGASignalToBlack();
	SetVGAHorizontalSyncActiveHighForDMA(BlankHSyncHandler350);
}

void SetBlankVGAScreenMode600()
{
	SetVGASignalToBlack();
	SetVGAHorizontalSyncActiveHighForDMA(BlankHSyncHandler600);
}

void SetBlankVGAScreenMode768()
{
	SetVGASignalToBlack();
	SetVGAHorizontalSyncForDMA(BlankHSyncHandler768);
}

static void BlankHSyncHandler480()
{
	HandleVGAHSync480();
}

static void BlankHSyncHandler400()
{
	HandleVGAHSync400();
}

static void BlankHSyncHandler350()
{
	HandleVGAHSync350();
}

static void BlankHSyncHandler600()
{
	HandleVGAHSync600();
}

static void BlankHSyncHandler768()
{
	HandleVGAHSync768();
}




static void PixelHSyncHandler240At480();
static void PixelHSyncHandler200At480();
static void PixelHSyncHandler200At400();
static void PixelHSyncHandler175At350();
static void PixelHSyncHandler160At480();
static void PixelHSyncHandler133At480();
static void PixelHSyncHandler133At400();
static void PixelHSyncHandler117At350();
static void PixelHSyncHandler128At768();

static void InitializePixelDMA(int pixelticks,int pixelsperrow);
static void DMACompleteHandler();
static inline void StartPixelDMA();
static inline void StopPixelDMA();

void SetVGAScreenMode240At480(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler240At480);
}

void SetVGAScreenMode200At480(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler200At480);
}

void SetVGAScreenMode200At400(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler200At400);
}

void SetVGAScreenMode175At350(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	SetVGAHorizontalSyncActiveHighForDMA(PixelHSyncHandler175At350);
}

void SetVGAScreenMode160At480(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	VGALineMultiplierCounter=0;
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler160At480);
}

void SetVGAScreenMode133At480(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	VGALineMultiplierCounter=0;
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler133At480);
}

void SetVGAScreenMode133At400(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	VGALineMultiplierCounter=0;
	SetVGAHorizontalSyncForDMA(PixelHSyncHandler133At400);
}

void SetVGAScreenMode117At350(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	VGALineMultiplierCounter=0;
	SetVGAHorizontalSyncActiveHighForDMA(PixelHSyncHandler117At350);
}

void SetVGAScreenMode128At768(uint8_t *framebuffer,int pixelsperrow,int pixelticks)
{
	WaitForLastLineIfVGAEnabled();
	InitializePixelDMA(pixelticks,pixelsperrow);
	SetFrameBuffer(framebuffer);
	VGALineMultiplierCounter=0;
	SetVGAHorizontalSyncActiveHighForDMA(PixelHSyncHandler128At768);
}

void SetFrameBuffer(uint8_t *framebuffer)
{
	VGAFrameBufferAddress=VGACurrentLineAddress=(uint32_t)framebuffer;
}

static void PixelHSyncHandler240At480()
{
	int line=HandleVGAHSync480();
	if(line<0) return;

	if(line==0) VGACurrentLineAddress=VGAFrameBufferAddress;
	StartPixelDMA();
	if(line&1) VGACurrentLineAddress+=VGAPixelsPerRow;
}

static void PixelHSyncHandler200At480()
{
	int line=HandleVGAHSync400At480();
	if(line<0) return;

	if(line==0) VGACurrentLineAddress=VGAFrameBufferAddress;
	StartPixelDMA();
	if(line&1) VGACurrentLineAddress+=VGAPixelsPerRow;
}

static void PixelHSyncHandler200At400()
{
	int line=HandleVGAHSync400();
	if(line<0) return;

	if(line==0) VGACurrentLineAddress=VGAFrameBufferAddress;
	StartPixelDMA();
	if(line&1) VGACurrentLineAddress+=VGAPixelsPerRow;
}

static void PixelHSyncHandler175At350()
{
	int line=HandleVGAHSync350();
	if(line<0) return;

	if(line==0) VGACurrentLineAddress=VGAFrameBufferAddress;
	StartPixelDMA();
	if(line&1) VGACurrentLineAddress+=VGAPixelsPerRow;
}

static void PixelHSyncHandler160At480()
{
	int line=HandleVGAHSync480();
	if(line<0) return;

	if(line==0)
	{
		VGACurrentLineAddress=VGAFrameBufferAddress;
		VGALineMultiplierCounter=0;
	}

	StartPixelDMA();

	if(VGALineMultiplierCounter++==2)
	{
		VGACurrentLineAddress+=VGAPixelsPerRow;
		VGALineMultiplierCounter=0;
	}
}

static void PixelHSyncHandler133At480()
{
	int line=HandleVGAHSync400At480();
	if(line<0) return;

	if(line==0)
	{
		VGACurrentLineAddress=VGAFrameBufferAddress;
		VGALineMultiplierCounter=0;
	}

	StartPixelDMA();

	if(VGALineMultiplierCounter++==2)
	{
		VGACurrentLineAddress+=VGAPixelsPerRow;
		VGALineMultiplierCounter=0;
	}
}

static void PixelHSyncHandler133At400()
{
	int line=HandleVGAHSync400();
	if(line<0) return;

	if(line==0)
	{
		VGACurrentLineAddress=VGAFrameBufferAddress;
		VGALineMultiplierCounter=0;
	}

	StartPixelDMA();

	if(VGALineMultiplierCounter++==2)
	{
		VGACurrentLineAddress+=VGAPixelsPerRow;
		VGALineMultiplierCounter=0;
	}
}

static void PixelHSyncHandler117At350()
{
	int line=HandleVGAHSync350();
	if(line<0) return;

	if(line==0)
	{
		VGACurrentLineAddress=VGAFrameBufferAddress;
		VGALineMultiplierCounter=0;
	}

	StartPixelDMA();

	if(VGALineMultiplierCounter++==2)
	{
		VGACurrentLineAddress+=VGAPixelsPerRow;
		VGALineMultiplierCounter=0;
	}
}

static void PixelHSyncHandler128At768()
{
	int line=HandleVGAHSync768();
	if(line<0) return;

	if(line==0)
	{
		VGACurrentLineAddress=VGAFrameBufferAddress;
		VGALineMultiplierCounter=0;
	}

	StartPixelDMA();

	if(VGALineMultiplierCounter++==5)
	{
		VGACurrentLineAddress+=VGAPixelsPerRow;
		VGALineMultiplierCounter=0;
	}
}

static void InitializePixelDMA(int pixelticks,int pixelsperrow)
{
	// Configure timer 8 as the pixel clock.
	TIM8->CR1=TIM_CR1_ARPE;
	TIM8->DIER=TIM_DIER_UDE; // Enable update DMA request.
	TIM8->PSC=0; // Prescaler = 1.
	TIM8->ARR=pixelticks-1;
	TIM8->SMCR=(5*TIM_SMCR_SMS_0)|(1*TIM_SMCR_TS_0); // Only run TIM8 when TIM2 trigger-out is high.

	// DMA2 stream 1 channel 7 is triggered by timer 8.
	// Stop it and configure interrupts.
	DMA2_Stream1->CR&=~DMA_SxCR_EN;
	InstallInterruptHandler(DMA2_Stream1_IRQn,DMACompleteHandler);
	EnableInterrupt(DMA2_Stream1_IRQn);
	SetInterruptPriority(DMA2_Stream1_IRQn,0);

	VGAPixelsPerRow=pixelsperrow;
}

static void DMACompleteHandler()
{
	SetVGASignalToBlack();
	DMA2->LIFCR|=DMA_LIFCR_CTCIF1; // Clear interrupt flag.
	StopPixelDMA();

	if(HBlankInterruptHandler) HBlankInterruptHandler();
	// TODO: VBlank interrupt? At lower priority?
}

static inline void StartPixelDMA()
{
	// Configure and enable pixel DMA.
	DMA2_Stream1->CR=(7*DMA_SxCR_CHSEL_0)| // Channel 7
	(3*DMA_SxCR_PL_0)| // Priority 3
	(0*DMA_SxCR_PSIZE_0)| // PSIZE = 8 bit
	(0*DMA_SxCR_MSIZE_0)| // MSIZE = 8 bit
	DMA_SxCR_MINC| // Increase memory address
	(1*DMA_SxCR_DIR_0)| // Memory to peripheral
	DMA_SxCR_TCIE| // Transfer complete interrupt
	(1*DMA_SxCR_MBURST_0); // burst on the memory-side
//	(2*DMA_SxCR_MSIZE_0) // perhaps 32-bit bursts on memory-side. Currently does not work.

	DMA2_Stream1->FCR=DMA_SxFCR_DMDIS| // Enable FIFO.
	(1*DMA_SxFCR_FTH_0); // Set threshold to 1/2. (TODO: look at this)

	DMA2_Stream1->NDTR=VGAPixelsPerRow;
	DMA2_Stream1->PAR=((uint32_t)&GPIOE->ODR)+1;
	DMA2_Stream1->M0AR=VGACurrentLineAddress;

	// Enable pixel clock. Clock will only start once TIM2 allows it.
	TIM8->DIER=0; // Update DMA request has to be disabled while zeroing the counter.
	TIM8->EGR=TIM_EGR_UG; // Force an update event to reset counter. Setting CNT is not reliable.
	TIM8->DIER=TIM_DIER_UDE; // Re-enable update DMA request.
	TIM8->CR1|=TIM_CR1_CEN;

	DMA2_Stream1->CR|=DMA_SxCR_EN;
}

static inline void StopPixelDMA()
{
	TIM8->CR1&=~TIM_CR1_CEN; // Stop pixel clock.
	DMA2_Stream1->CR=0; // Disable pixel DMA.
}