#include <avr/io.h>
#include "def.h"
#define F_CPU	8000000UL
#include <util/delay.h>

#include <avr/interrupt.h>
#include <string.h>
#include <inttypes.h>
#include <util/twi.h>

/* other macros */
// macro for PS-AVR SPI communication
#define PS_AVR_COMM(avrdata, pscommand) {\
	if(COMM_OK_CHECK){\
		START_TIMER_AT(200);\
		SPDR = ~avrdata;\
		while((!(SPSR & _BV(SPIF))) && (!(TIFR2 & _BV(TOV2))));\
		(TIFR2 & _BV(TOV2)) ? (COMM_IS_NOT_OK) : (pscommand = SPDR);\
		SPI_PORT &= ~_BV(MISO);\
		}\
	}

// SPI acknowledge macro, sends a 2us low ACK pulse following every SPI byte received, not sent following final byte in packet
#define ACKNOWLEDGE {\
		if(COMM_OK_CHECK){\
		SPI_PORT |= _BV(ACK);\
		_delay_us(3);\
		SPI_PORT &= ~_BV(ACK);\
	}\
}

// NES controller read macro, reads controllers connected to player1 and player2 ports, gets one byte for each containing 8 button states 
#define NES_GET_DATA(player1_data, player2_data) {\
	player1_data = 0xFF;\
	player2_data = 0xFF;\
	NES_PORT |= _BV(NES_LATCH);\
	_delay_us(12);\
	NES_PORT &= ~_BV(NES_LATCH);\
	_delay_us(6);\
	for(uint8_t bit_counter = 0; bit_counter < 8; bit_counter++){\
		(NES_DATA_PIN & _BV(NES_DATA0)) ? (player1_data |= _BV(bit_counter)) : (player1_data &= ~_BV(bit_counter));\
		(NES_DATA_PIN & _BV(NES_DATA1)) ? (player2_data |= _BV(bit_counter)) : (player2_data &= ~_BV(bit_counter));\
		NES_PORT |= _BV(NES_CLOCK);\
		_delay_us(6);\
		NES_PORT &= ~_BV(NES_CLOCK);\
		_delay_us(6);\
	}\
}

// set shift register with 8-bit value from a given register
#define SET_SHIFT_REGISTER(byte_to_shift) {\
		uint8_t bit_counter = 8;\
		do{\
			bit_counter--;\
			(byte_to_shift & _BV(bit_counter)) ? (SHIFT_REG_PORT |= _BV(SHIFT_REG_DATA)) : (SHIFT_REG_PORT &= ~_BV(SHIFT_REG_DATA));\
			SHIFT_REG_PORT |= _BV(SHIFT_REG_CLOCK);\
			SHIFT_REG_PORT &= ~_BV(SHIFT_REG_CLOCK);\
		}while(bit_counter > 0);\
		SHIFT_REG_PORT |= _BV(SHIFT_REG_LATCH);\
		SHIFT_REG_PORT &= ~_BV(SHIFT_REG_LATCH);\
}

// write 1 to TOV2 bit of TIFR2 to clear flag; counter starts at value "x", counts up to 255 then overflows and sets TOV2
#define START_TIMER_AT(x) {\
	TIFR2 |= _BV(TOV2);\
	TCNT2 = x;\
}

// map a button with turbo (rapid fire)
#define TURBO_MAP(data_byte_in, data_byte_out, button_in, button_out, turbo_register, active_bit, toggle_bit) {\
		 if(!(data_byte_in & _BV(button_in))){\
			if(!(turbo_register & _BV(active_bit))){ data_byte_out &= ~_BV(button_out); turbo_register |= _BV(active_bit);}\
			else if((!(turbo_register & _BV(toggle_bit))) && (turbo_counter == 0)){ data_byte_out |= _BV(button_out); turbo_register |= _BV(toggle_bit);}\
			else if((turbo_register & _BV(toggle_bit)) && (turbo_counter == 0)){ data_byte_out &= ~_BV(button_out); turbo_register &= ~_BV(toggle_bit);}\
			}\
		else{ turbo_register &= ~_BV(active_bit); turbo_register &= ~_BV(toggle_bit);}\
}

/* init() sets all registers and enables necessary interrupts */
void init(void){

	/*Set pin I/O registers*/
	// Bit-banged SPI pin setup
	BB_DDR |= _BV(COMMAND) | _BV(CLOCK) | _BV(ATTENTION);
	
	// NES pin setup
	NES_DDR |= _BV(NES_LATCH) | _BV(NES_CLOCK);
	NES_PORT |= _BV(NES_DATA0) | _BV(NES_DATA1);		// Enable pull-up resistor on NES data pin

	// saturn controller pin setup
	SAT_DDR |= _BV(SAT_SELECT0) | _BV(SAT_SELECT1);
	SAT_PORT |= _BV(SAT_DATA0) | _BV(SAT_DATA1) | _BV(SAT_DATA2) | _BV(SAT_DATA3); // enable pull-up resistors on data pins

	// Hardware SPI pin setup 
	SPI_DDR |= _BV(ACK) | _BV(MISO);
	SPI_PORT |= _BV(SS);	// Enable pull-up resistor on SS pin

	// Status LED pin setup
	LED_DDR |= _BV(STATUS_LED_MOTOR) | _BV(STATUS_LED_GREEN) | _BV(STATUS_LED_RED2) | _BV(STATUS_LED_GREEN2);
	LED_PORT |= _BV(STATUS_LED_MOTOR) | _BV(STATUS_LED_GREEN) | _BV(STATUS_LED_RED2) | _BV(STATUS_LED_GREEN2);

	// TWI pin setup
	TWI_PORT |= _BV(SCL) | _BV(SDA);	// enable pull-up resistors on TWI communication pins

	// shift register pin setup
	SHIFT_REG_DDR |= _BV(SHIFT_REG_DATA) | _BV(SHIFT_REG_CLOCK) | _BV(SHIFT_REG_LATCH);

	// interface button setup
	ANA_DIG_SELECT_PORT |= _BV(ANA_DIG_SELECT);	// Enable pull-up resistor on analog/digital select pin
	CONFIG_CYCLE_PORT |= _BV(CONFIG_CYCLE);		// Enable pull-up resistor on configuration cycle pin

	// Debugging DDR setup
	//DEBUG_DDR |= _BV(DEBUG);

	/* Configure hardware SPI registers (pg.173) */
	// SPE enables SPI hardware; DORD sets data order to LSB first; CPOL sets clock polarity (high when idle)
	// CPHA selects data setup on falling edge of clock, read on rising edge
	SPCR |= _BV(SPE) | _BV(DORD) | _BV(CPOL) | _BV(CPHA);

	/*Set up 8-bit timer/counter 2*/
	// Timer/Counter 0 Control Register B : Prescaler set to divide system clock (8mHz) by 32 for timer clock
	TCCR2B	|= _BV(CS20) | _BV(CS21);
	// Write 0x01 to TCNT2 to reset timer, check for TOV2 bit in TIFR2 for timer overflow.

	/* configure USART registers */
	// equations for calculating and setting UBRR0L (baudrate register) (pg. 179)
	if((F_CPU / 16 / BAUDRATE - 1) < 20){
  	UCSR0A |= _BV(U2X0);		
  	UBRR0L = F_CPU / 8 / BAUDRATE - 1;}
  	else{
  	UBRR0L = F_CPU / 16 / BAUDRATE - 1;}
	// USART control and status register C (pg. 197); UCSZ01 and UCSZ00 sets character size to 8 bits;
	UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);
	// UCSR0B (pg. 196); RXCIE0 enables RX complete interrupt; TXEN0 and RXEN0 enable receiver and transmitter 
	UCSR0B |= _BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0);

	/* TWI setup */
	TWBR = 0x16;				// SCL frequency = 166kHz (CPU_FREQ / 16 + (2 * TWBR) * (4^ prescaler bits))
}

/* writes a single instruction byte to character lcd through shift register, cuts instruction into two nibbles for 4-bit mode */
void lcd_instruction_write(uint8_t instruction){

	uint8_t shift_reg_out = 0x00;
	shift_reg_out &= ~_BV(SHIFT_REG_RS_BIT);				// set RS pin low to select instruction register
	shift_reg_out |= _BV(SHIFT_REG_E_BIT);					// set E pin high before data is prepared
	shift_reg_out = ((shift_reg_out & 0xF0) | ((instruction >> 4) & 0x0F));	// right shift upper nibble of instruction to write first
	SET_SHIFT_REGISTER(shift_reg_out);					// shift_reg_out byte is now present on shift register outputs
	shift_reg_out &= ~_BV(SHIFT_REG_E_BIT);					// set E pin low to write high nibble of instruction into register
	SET_SHIFT_REGISTER(shift_reg_out);
	shift_reg_out |= _BV(SHIFT_REG_E_BIT);					// set E pin high before data is prepared
	shift_reg_out = ((shift_reg_out & 0xF0) | (instruction & 0x0F));	// then send out lower nibble of instruction byte
	SET_SHIFT_REGISTER(shift_reg_out);
	shift_reg_out &= ~_BV(SHIFT_REG_E_BIT);					// set E pin low to write low nibble of instruction into register
	SET_SHIFT_REGISTER(shift_reg_out);
	_delay_us(20);
}

/* writes a single data byte to character lcd through shift register, cuts data into two nibbles for 4-bit mode */	
void lcd_data_write(uint8_t data){

	uint8_t shift_reg_out = 0x00;
	shift_reg_out |= _BV(SHIFT_REG_RS_BIT) | _BV(SHIFT_REG_E_BIT);		//RS pin high for data register, E pin high before data is set
	shift_reg_out = ((shift_reg_out & 0xF0) | ((data >> 4) & 0x0F));	// right shift upper nibble of data byte to write first
	SET_SHIFT_REGISTER(shift_reg_out);					// shift_reg_out byte is now present on shift register outputs
	shift_reg_out &= ~_BV(SHIFT_REG_E_BIT);					// set E pin low to write high nibble of data into register
	SET_SHIFT_REGISTER(shift_reg_out);
	shift_reg_out |= _BV(SHIFT_REG_E_BIT);					// set E pin high before data is prepared
	shift_reg_out = ((shift_reg_out & 0xF0) | (data & 0x0F));		// then send out lower nibble of data byte
	SET_SHIFT_REGISTER(shift_reg_out);
	shift_reg_out &= ~_BV(SHIFT_REG_E_BIT);					// set E pin low to write low nibble of data into register
	SET_SHIFT_REGISTER(shift_reg_out);
	_delay_us(20);
}

/* writes a string of characters to lcd at current position, stops when it hits the null character (\0) */
void lcd_string_print(char string[]){

	uint8_t char_counter = 0;			// increments char to write from string

	while(string[char_counter] != '\0'){		// while char_counter does not equal the terminating null at the end of string
		lcd_data_write(string[char_counter]);	// write a single char to lcd
		char_counter++;				// move to next char in string
	}
}

/* sets lcd address counter to specified location */
void lcd_move_to_address(uint8_t address){

	lcd_instruction_write(0x80 + address);		// adds 7 bit address to instruction 0x80; addresses 0x00 (0) thorough 0x4F (79) are valid
}

/* initialize and configure the character lcd, runs once at startup */
void lcd_init(void){

	SET_SHIFT_REGISTER(0x0F);
        _delay_ms(50);
        lcd_instruction_write(DISPLAY_OFF);	// turns display off without clearing
	_delay_ms(2);
	lcd_instruction_write(CLEAR_SCREEN);	// clears display
	_delay_ms(2);
	lcd_instruction_write(RETURN_HOME);	// returns posistion to top left of display area
	_delay_ms(2);
        lcd_instruction_write(FOUR_BIT_2_LINE);	// 4-bit mode - 2 line - 5x7 characters
        lcd_instruction_write(HIDE_CURSOR);	// hides blinking block cursor
        lcd_instruction_write(0x06);		// sets character entry mode; display shift off, increment address counter after each character sent
        lcd_move_to_address(0);			// set DDRAM address to 0
}

/* USART RS232 transmit single byte function */
void avr_transmit_byte(uint8_t data) {
	while (!(UCSR0A & _BV(UDRE0))); 	// Wait for USART data register empty flag (pg. 195)
	UDR0 = data;				// Write byte to USART I/O data register to initiate transmission (p. 195)
}

/* Core AVR-DSC send/receive communication function, sends command_byte, recieves and returns ps2_return_byte */
uint8_t ps2_comm(uint8_t command_byte){				

	uint8_t ps2_return_byte = 0;					//variable to store data returned from DSC
	
	for(uint8_t bit_counter = 0; bit_counter < 8; bit_counter++){	//for loop cycles 8 times for one full byte

		if(command_byte & _BV(bit_counter))			//if a 1 exists in the nth bit place of command_byte
			BB_PORT |= _BV(COMMAND);			//output a 1 from AVR->DSC
		else BB_PORT &= ~_BV(COMMAND);				//if a 0 exists, write 0 instead
		
		BB_PORT &= ~_BV(CLOCK);					//drop clock line low
		_delay_us(4);						//delay while clock settles

		if(BB_PIN & _BV(DATA))					//if a 1 is read on data line from DSC
			ps2_return_byte |= _BV(bit_counter);		//place 1 in nth bit of ps2_return_byte
		
		BB_PORT |= _BV(CLOCK);					//clock returns to high state, DSC reads command bit at this time
		_delay_us(4);
	}

	_delay_us(16);							//delay after 1 full byte has been sent/received
	return(ps2_return_byte);					//return data from DSC
}

/* Function to simplify configuration of DSC; requires 4 command bytes, loop_bytes sets number of bytes following the default 6 */
uint8_t config_comm(uint8_t byte2, uint8_t byte4, uint8_t byte5, uint8_t byte_to_loop, uint8_t loop_byte_count){

	uint8_t controller_mode_ID = 0x00;
	BB_PORT &= ~_BV(ATTENTION);
	
	_delay_us(16);



	ps2_comm(0x01);

	controller_mode_ID = ps2_comm(byte2);

	ps2_comm(0x00);

	ps2_comm(byte4);

	ps2_comm(byte5);
	
	for(uint8_t x = 0; x < loop_byte_count; x++)
		ps2_comm(byte_to_loop);	
		
	_delay_us(16);


	BB_PORT |= _BV(ATTENTION);
	return(controller_mode_ID);
}

/* function for reading data from saturn controller */
uint8_t saturn_get_first_byte(void){
	
	uint8_t sat_data = 0xFF;
	
	SAT_PORT &= ~_BV(SAT_SELECT0);
	SAT_PORT &= ~_BV(SAT_SELECT1);
	_delay_us(10);

	(~SAT_PIN & _BV(SAT_DATA0)) ? (sat_data &= ~_BV(SAT_Z)) : (sat_data |= _BV(SAT_Z));
	(~SAT_PIN & _BV(SAT_DATA1)) ? (sat_data &= ~_BV(SAT_Y)) : (sat_data |= _BV(SAT_Y));
	(~SAT_PIN & _BV(SAT_DATA2)) ? (sat_data &= ~_BV(SAT_X)) : (sat_data |= _BV(SAT_X));
	(~SAT_PIN & _BV(SAT_DATA3)) ? (sat_data &= ~_BV(SAT_R)) : (sat_data |= _BV(SAT_R));
	
	SAT_PORT |= _BV(SAT_SELECT0);
	SAT_PORT &= ~ _BV(SAT_SELECT1);
	_delay_us(10);

	(~SAT_PIN & _BV(SAT_DATA0)) ? (sat_data &= ~_BV(SAT_B)) : (sat_data |= _BV(SAT_B));
	(~SAT_PIN & _BV(SAT_DATA1)) ? (sat_data &= ~_BV(SAT_C)) : (sat_data |= _BV(SAT_C));
	(~SAT_PIN & _BV(SAT_DATA2)) ? (sat_data &= ~_BV(SAT_A)) : (sat_data |= _BV(SAT_A));
	(~SAT_PIN & _BV(SAT_DATA3)) ? (sat_data &= ~_BV(SAT_START)) : (sat_data |= _BV(SAT_START));
	
	return(sat_data);
}

uint8_t saturn_get_second_byte(void){
	
	uint8_t sat_data = 0xFF;

	SAT_PORT &= ~_BV(SAT_SELECT0);
	SAT_PORT |= _BV(SAT_SELECT1);
	_delay_us(10);

	(~SAT_PIN & _BV(SAT_DATA0)) ? (sat_data &= ~_BV(SAT_UP)) : (sat_data |= _BV(SAT_UP));
	(~SAT_PIN & _BV(SAT_DATA1)) ? (sat_data &= ~_BV(SAT_DOWN)) : (sat_data |= _BV(SAT_DOWN));
	(~SAT_PIN & _BV(SAT_DATA2)) ? (sat_data &= ~_BV(SAT_LEFT)) : (sat_data |= _BV(SAT_LEFT));
	(~SAT_PIN & _BV(SAT_DATA3)) ? (sat_data &= ~_BV(SAT_RIGHT)) : (sat_data |= _BV(SAT_RIGHT));

	SAT_PORT |= _BV(SAT_SELECT0);
	SAT_PORT |= _BV(SAT_SELECT1);
	_delay_us(10);

	(~SAT_PIN & _BV(SAT_DATA3)) ? (sat_data &= ~_BV(SAT_L)) : (sat_data |= _BV(SAT_L));
	
	return(sat_data);
}

/* Functions for remapping buttons between controllers */
void OR_map(uint8_t source_byte, uint8_t *destination_byte, uint8_t source_bit, uint8_t destination_bit){

	if(~source_byte & _BV(source_bit))
		*destination_byte &= ~_BV(destination_bit);
	}

void map_if_less(uint8_t source_byte, uint8_t *destination_byte, uint8_t compare_value, uint8_t write_value){

	if(source_byte < compare_value)
		*destination_byte = write_value;
	}

void map_if_greater(uint8_t source_byte, uint8_t *destination_byte, uint8_t compare_value, uint8_t write_value){

	if(source_byte > compare_value)
		*destination_byte = write_value;
	}

void map_a2d_if_less(uint8_t source_byte, uint8_t *destination_byte, uint8_t compare_value, uint8_t destination_bit){

	if(source_byte < compare_value)
		*destination_byte &= ~_BV(destination_bit);
	}

void map_a2d_if_greater(uint8_t source_byte, uint8_t *destination_byte, uint8_t compare_value, uint8_t destination_bit){

	if(source_byte > compare_value)
		*destination_byte &= ~_BV(destination_bit);
	}

/* bring a value within a given range into 8-bit range (0 - 255) */
uint8_t range_8bit(uint8_t x, uint8_t in_min, uint8_t in_max){

	long out = ((x - in_min) * 255) / (in_max - in_min);

	if(out > 255){
		out = 255;}
	else if(out < 0){
		out = 0;}
	return(out);
}

/* send start condition to initiate activity on TWI bus */
void twi_start(void){
	SEND_TWI_START;
	WAIT_FOR_TWINT(200);		// argument 242 starts timer counting up from 242, overflows in approx. 50us  
	}

/* send slave address with read/write bit set to 0 to setup data write from master to slave */
void twi_send_slaw(uint8_t slave_address){
	TWDR = ((slave_address << 1) + TW_WRITE);	// left shifts 7 bit address and inserts W bit in LSB for SLA+W
	TWCR = _BV(TWINT) | _BV(TWEN);
	WAIT_FOR_TWINT(200);
	}

/* send slave address with read/write bit set to 1 to setup data read from slave to master */
void twi_send_slar(uint8_t slave_address){
	TWDR = ((slave_address << 1) + TW_READ);	// left shifts 7 bit address and inserts R bit in LSB for SLA+R
	TWCR = _BV(TWINT) | _BV(TWEN);
	WAIT_FOR_TWINT(200);
	}

/* write a single data byte from twi master to twi slave */
void twi_write_data(uint8_t byte_to_write){
	TWDR = byte_to_write;
	TWCR = _BV(TWINT) | _BV(TWEN);
	WAIT_FOR_TWINT(200);
	}

/* read a single data byte from twi slave to twi master */
// takes argument of "1" or "0" to determine whether ACK is sent; "1" = ACK, "0" = NACK; returns data byte from TWDR
uint8_t twi_read_data(uint8_t ack_bit){
	ack_bit ? (TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA)) : (TWCR = _BV(TWINT) | _BV(TWEN));
	WAIT_FOR_TWINT(200);
	return(TWDR);
}