#include "all-headers.h"


#include <stddef.h>
#include <stdbool.h>


// http://esd.cs.ucr.edu/labs/interface/interface.html

// PORT CONFIGURATION                                                                                                *
// LCD D4 : Teensy D16
// LCD D5 : Teensy D15
// LCD D6 : Teensy D14
// LCD D7 : Teensy D19
// LCD RS : Teensy D18
// LCD E  : Teensy D17

static const DigitalPort LCD_D4 = DigitalPort::D16 ;

static const DigitalPort LCD_D5 = DigitalPort::D15 ;

static const DigitalPort LCD_D6 = DigitalPort::D14 ;

static const DigitalPort LCD_D7 = DigitalPort::D19 ;

static const DigitalPort LCD_RS = DigitalPort::D18 ;

static const DigitalPort LCD_E  = DigitalPort::D17 ;

// UTILITY ROUTINES — ANY MODE                                                                                                  *

static void driveHighE (void) {
	digitalWrite (LCD_E, true) ;
}


static void driveLowE (void) {
	digitalWrite (LCD_E, false) ;
}


static void driveHighRS (void) {
	digitalWrite (LCD_RS, true) ;
}


static void driveLowRS (void) {
	digitalWrite (LCD_RS, false) ;
}


static void setD4 (const bool inValue) {
	digitalWrite (LCD_D4, inValue) ;
}


static void setD5 (const bool inValue) {
	digitalWrite (LCD_D5, inValue) ;
}


static void setD6 (const bool inValue) {
	digitalWrite (LCD_D6, inValue) ;
}


static void setD7 (const bool inValue) {
	digitalWrite (LCD_D7, inValue) ;
}


static void programLcd4BitDataBusOutput (const uint8_t inValue) {
	setD4 ((inValue & 0x01) != 0) ;
	setD5 ((inValue & 0x02) != 0) ;
	setD6 ((inValue & 0x04) != 0) ;
	setD7 ((inValue & 0x08) != 0) ;
}

// UTILITY ROUTINES — INIT_MODE

static void write4BitCommand_initMode (INIT_MODE_ const uint8_t inCommand) {
	busyWaitDuring_initMode (MODE_ 1) ;
	driveLowRS () ;
	programLcd4BitDataBusOutput (inCommand) ;
	driveHighE () ;
	busyWaitDuring_initMode (MODE_ 1) ;
	driveLowE () ;
}


static void write8bitCommand_initMode (INIT_MODE_ const uint8_t inCommand) {
	busyWaitDuring_initMode (MODE_ 1) ;
	driveLowRS () ;
	programLcd4BitDataBusOutput ((uint8_t) (inCommand >> 4)) ;
	driveHighE () ;
	busyWaitDuring_initMode (MODE_ 1) ;
	driveLowE () ;
	busyWaitDuring_initMode (MODE_ 1) ;
	programLcd4BitDataBusOutput (inCommand) ;
	driveHighE () ;
	busyWaitDuring_initMode (MODE_ 1) ;
	driveLowE () ;
}

// LCD INIT

static void setupLCD (INIT_MODE) {
	// Step 1: Configure ports
	pinMode (LCD_D4, DigitalMode::OUTPUT) ;
	pinMode (LCD_D5, DigitalMode::OUTPUT) ;
	pinMode (LCD_D6, DigitalMode::OUTPUT) ;
	pinMode (LCD_D7, DigitalMode::OUTPUT) ;
	pinMode (LCD_RS, DigitalMode::OUTPUT) ;
	pinMode (LCD_E,  DigitalMode::OUTPUT) ;
	// Step 2: wait for 15 ms
	busyWaitDuring_initMode (MODE_ 15) ;
	// Step 3: write command 0x30
	write4BitCommand_initMode (MODE_ 0x3) ;
	// Step 4: wait for 4,1 ms (actually 5 ms)
	busyWaitDuring_initMode (MODE_ 5) ;
	// Step 5: write command 0x30 again
	write4BitCommand_initMode (MODE_ 0x3) ;
	// Step 6: wait for 100 µs (actually 1 ms)
	busyWaitDuring_initMode (MODE_ 1) ;
	// Step 7: write command 0x30 (third)
	write4BitCommand_initMode (MODE_ 0x3) ;
	// Step 8: write command 0x20 (4-bit mode)
	write4BitCommand_initMode (MODE_ 0x2) ;
	// Step 9: write command 'Set Interface Length' : 0 0 1 DL N F * *
	// DL : Data interface length : 0 (4 bits)
	// N : Number of Display lines : 1 (2 lines)
	// F : Character Font : 0 (5x7)
	write8bitCommand_initMode (MODE_ 0x28) ;
	// Step 10: write command 'Display Off'
	write8bitCommand_initMode (MODE_ 0x08) ;
	// Step 11: write command 'Clear Display'
	write8bitCommand_initMode (MODE_ 0x01) ;
	// Step 12: write command 'Set Cursor Move Direction' : 0 0 0 0 0 1 ID S
	// ID : Increment Cursor after Each Byte Written to Display : 1 (yes)
	// S : Shift Display When Byte Written : 0 (no)
	write8bitCommand_initMode (MODE_ 0x06) ;
	// Step 13: write command 'Move Cursor / Shift Display' : 0 0 0 1 SC RL * *
	// SC : Display Shift On : 1 (oui)
	// RL : Direction of Shift : 1 (to right)
	write8bitCommand_initMode (MODE_ 0x1C) ;
	// Step 14: write command 'Return Cursor and LCD to Home Position'
	write8bitCommand_initMode (MODE_ 0x02) ;
	// Step 15: write command 'Enable Display / Cursor' : 0 0 0 0 1 D C B
	// D : Turn Display On : 1 (yes)
	// C : Turn Cursor On : 0 (no)
	// B : Cursor Blink On : 0 (no)
	write8bitCommand_initMode (MODE_ 0x0C) ;
}


MACRO_INIT_ROUTINE (setupLCD) ;

// UTILITY ROUTINES — USER MODE

static void write8bitCommand (USER_MODE_ const uint8_t inCommand) {
	waitDuring (MODE_ 1) ;
	driveLowRS () ;
	programLcd4BitDataBusOutput ((uint8_t) (inCommand >> 4)) ;
	driveHighE () ;
	waitDuring (MODE_ 1) ;
	driveLowE () ;
	waitDuring (MODE_ 1) ;
	programLcd4BitDataBusOutput (inCommand) ;
	driveHighE () ;
	waitDuring (MODE_ 1) ;
	driveLowE () ;
}


static void writeData (USER_MODE_ const uint8_t inData) {
	waitDuring (MODE_ 1) ;
	driveHighRS () ;
	programLcd4BitDataBusOutput (inData >> 4) ;
	driveHighE () ;
	waitDuring (MODE_ 1) ;
	driveLowE () ;
	waitDuring (MODE_ 1) ;
	programLcd4BitDataBusOutput (inData) ;
	driveHighE () ;
	waitDuring (MODE_ 1) ;
	driveLowE () ;
}

// PRINT ROUTINES — USER MODE

void clearScreen (USER_MODE) {
	write8bitCommand (MODE_ 0x01) ;
}

// Line 0 : 00 -> 19
// Line 1 : 64 -> 83
// Line 2 : 20 -> 39
// Line 3 : 84 -> 103

void gotoLineColumn (USER_MODE_ const uint32_t inLine, const uint32_t inColumn) {
	static const uint8_t tab [4] = {0, 64, 20, 84} ;
	if ((inLine < 4) && (inColumn < 20)) {
		write8bitCommand (MODE_ tab [inLine] + inColumn + 0x80U) ;
	}
}


void printString (USER_MODE_ const char * inString) {
	if (NULL != inString) {
		while ('\0' != *inString) {
			writeData (MODE_ *inString) ;
			inString ++ ;
		}
	}
}


void printChar (USER_MODE_ const char inChar) {
	writeData (MODE_ inChar) ;
}


void printSpaces (USER_MODE_ const uint32_t inCount) {
	uint32_t count = inCount ;
	while (count > 0) {
		printChar (MODE_ ' ') ;
		count -- ;
	}
}


void printUnsigned (USER_MODE_ const uint32_t inValue) {
	uint32_t divisor = 1000 * 1000 * 1000 ; // 10**9
	uint32_t value = inValue ;
	bool isPrinting = false ;
	while (divisor > 0) {
		if (isPrinting || (value >= divisor)) {
			printChar (MODE_ '0' + value / divisor) ;
			value %= divisor ;
			isPrinting = true ;
		}
		divisor /= 10 ;
	}
	if (!isPrinting) {
		printChar (MODE_ '0') ;
	}
}


void printUnsigned64 (USER_MODE_ const uint64_t inValue) {
	char buffer [20] ;
	buffer [19] = '\0' ;
	buffer [18] = (inValue % 10) + '0' ;
	uint32_t idx = 18 ;
	uint64_t v = inValue / 10 ;
	while (v != 0) {
		idx -- ;
		buffer [idx] = (v % 10) + '0' ;
		v /= 10 ;
	}
	printString (MODE_ & buffer [idx]) ;
}


void printSigned (USER_MODE_ const int32_t inValue) {
	if (inValue < 0) {
		printChar (MODE_ '-') ;
		printUnsigned (MODE_ (uint32_t) -inValue) ;
	}else{
		printUnsigned (MODE_ (uint32_t) inValue) ;
	}
}


void printHex1 (USER_MODE_ const uint32_t inValue) {
	const uint32_t v = inValue & 0xF ;
	if (v < 10) {
		printChar (MODE_ '0' + v) ;
	}else{
		printChar (MODE_ 'A' + v - 10) ;
	}
}


void printHex2 (USER_MODE_ const uint32_t inValue) {
	printHex1 (MODE_ inValue >> 4) ;
	printHex1 (MODE_ inValue) ;
}


void printHex4 (USER_MODE_ const uint32_t inValue) {
	printHex2 (MODE_ inValue >> 8) ;
	printHex2 (MODE_ inValue) ;
}


void printHex8 (USER_MODE_ const uint32_t inValue) {
	printHex4 (MODE_ inValue >> 16) ;
	printHex4 (MODE_ inValue) ;
}


void printHex16 (USER_MODE_ const uint64_t inValue) {
	printHex8 (MODE_ (uint32_t) (inValue >> 32)) ;
	printHex8 (MODE_ (uint32_t) inValue) ;
}

// FAULT MODE

static void write4BitCommand_faultMode (FAULT_MODE_ const uint8_t inCommand) {
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowRS () ;
	programLcd4BitDataBusOutput (inCommand) ;
	driveHighE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowE () ;
}


static void write8bitCommand_faultMode (FAULT_MODE_ const uint8_t inCommand) {
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowRS () ;
	programLcd4BitDataBusOutput ((uint8_t) (inCommand >> 4)) ;
	driveHighE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	programLcd4BitDataBusOutput (inCommand) ;
	driveHighE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowE () ;
}


void initScreen_faultMode (FAULT_MODE) {
	// Step 1: Configure ports
	pinMode (LCD_D4, DigitalMode::OUTPUT) ;
	pinMode (LCD_D5, DigitalMode::OUTPUT) ;
	pinMode (LCD_D6, DigitalMode::OUTPUT) ;
	pinMode (LCD_D7, DigitalMode::OUTPUT) ;
	pinMode (LCD_RS, DigitalMode::OUTPUT) ;
	pinMode (LCD_E,  DigitalMode::OUTPUT) ;
	// Step 2: wait for 15 ms
	busyWaitDuring_faultMode (MODE_ 15) ;
	// Step 3: write command 0x30
	write4BitCommand_faultMode (MODE_ 0x3) ;
	// Step 4: wait for 4,1 ms (actually 5 ms)
	busyWaitDuring_faultMode (MODE_ 5) ;
	// Step 5: write command 0x30 again
	write4BitCommand_faultMode (MODE_ 0x3) ;
	// Step 6: wait for 100 µs (actually 1 ms)
	busyWaitDuring_faultMode (MODE_ 1) ;
	// Step 7: write command 0x30 (third)
	write4BitCommand_faultMode (MODE_ 0x3) ;
	// Step 8: write command 0x20 (4-bit mode)
	write4BitCommand_faultMode (MODE_ 0x2) ;
	// Step 9: write command 'Set Interface Length' : 0 0 1 DL N F * *
	// DL : Data interface length : 0 (4 bits)
	// N : Number of Display lines : 1 (2 lines)
	// F : Character Font : 0 (5x7)
	write8bitCommand_faultMode (MODE_ 0x28) ;
	// Step 10: write command 'Display Off'
	write8bitCommand_faultMode (MODE_ 0x08) ;
	// Step 11: write command 'Clear Display'
	write8bitCommand_faultMode (MODE_ 0x01) ;
	// Step 12: write command 'Set Cursor Move Direction' : 0 0 0 0 0 1 ID S
	// ID : Increment Cursor after Each Byte Written to Display : 1 (yes)
	// S : Shift Display When Byte Written : 0 (no)
	write8bitCommand_faultMode (MODE_ 0x06) ;
	// Step 13: write command 'Move Cursor / Shift Display' : 0 0 0 1 SC RL * *
	// SC : Display Shift On : 1 (oui)
	// RL : Direction of Shift : 1 (to right)
	write8bitCommand_faultMode (MODE_ 0x1C) ;
	// Step 14: write command 'Return Cursor and LCD to Home Position'
	write8bitCommand_faultMode (MODE_ 0x02) ;
	// Step 15: write command 'Enable Display / Cursor' : 0 0 0 0 1 D C B
	// D : Turn Display On : 1 (yes)
	// C : Turn Cursor On : 0 (no)
	// B : Cursor Blink On : 0 (no)
	write8bitCommand_faultMode (MODE_ 0x0C) ;
}


void gotoLineColumn_faultMode (FAULT_MODE_ const uint32_t inLine, const uint32_t inColumn) {
	static const uint8_t tab [4] = {0, 64, 20, 84} ;
	if ((inLine < 4) && (inColumn < 20)) {
		write8bitCommand_faultMode (MODE_ tab [inLine] + inColumn + 0x80U) ;
	}
}


static void writeData_faultMode (FAULT_MODE_ const uint8_t inData) {
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveHighRS () ;
	programLcd4BitDataBusOutput (inData >> 4) ;
	driveHighE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	programLcd4BitDataBusOutput (inData) ;
	driveHighE () ;
	busyWaitDuring_faultMode (MODE_ 1) ;
	driveLowE () ;
}


void printString_faultMode (FAULT_MODE_ const char * inString) {
	if (NULL != inString) {
		while ('\0' != *inString) {
			writeData_faultMode (MODE_ *inString) ;
			inString ++ ;
		}
	}
}


void printChar_faultMode (FAULT_MODE_ const char inChar) {
	writeData_faultMode (MODE_ inChar) ;
}


void printUnsigned_faultMode (FAULT_MODE_ const uint32_t inValue) {
	uint32_t divisor = 1000 * 1000 * 1000 ; // 10**9
	uint32_t value = inValue ;
	bool isPrinting = false ;
	while (divisor > 0) {
		if (isPrinting || (value >= divisor)) {
			printChar_faultMode (MODE_ '0' + value / divisor) ;
			value %= divisor ;
			isPrinting = true ;
		}
		divisor /= 10 ;
	}
	if (!isPrinting) {
		printChar_faultMode (MODE_ '0') ;
	}
}


static void printHex1_faultMode (FAULT_MODE_ const uint32_t inValue) {
	const uint32_t v = inValue & 0xF ;
	if (v < 10) {
		printChar_faultMode (MODE_ '0' + v) ;
	}else{
		printChar_faultMode (MODE_ 'A' + v - 10) ;
	}
}


void printHex2_faultMode (FAULT_MODE_ const uint32_t inValue) {
	printHex1_faultMode (MODE_ inValue >> 4) ;
	printHex1_faultMode (MODE_ inValue) ;
}


void printHex4_faultMode (FAULT_MODE_ const uint32_t inValue) {
	printHex2_faultMode (MODE_ inValue >> 8) ;
	printHex2_faultMode (MODE_ inValue) ;
}


void printHex8_faultMode (FAULT_MODE_ const uint32_t inValue) {
	printHex4_faultMode (MODE_ inValue >> 16) ;
	printHex4_faultMode (MODE_ inValue) ;
}