#include <avr/io.h>
#include <avr/interrupt.h>
#include "comm.hpp"

#define FOSC 1843200 // Clock Speed
#define SCLrate 100000 // TWI SCL frequency
#define SCLfreq (((FOSC/SCLrate)-16)/2) // Value for TVBR register (TWI)
#define BAUD 9600 // UART baud rate
#define BRREGVAL FOSC/16/BAUD-1 // Value for UBRR0H/L register (UART)

#define UART_ADDRESS 0x01
#define TWI_ADDRESS 0x01

#define Data_Direction PORTC3
#define XSDN1 (1<<DDD5)
#define XSDN2 (1<<DDD7)
#define XSDN3 (1<<DDD6)
#define TOF(XSDN) (DDRD |= XSDN)
#define TON(XSDN) (DDRD &= (~(XSDN)))

void TWI_init (void)
{
    TWSR = 0x00;
    TWBR = SCLfreq; // Set bit rate to 100kHz (for defined CPU clock speed)
    TWCR = (1<<TWEN);
}

void TWI_start(void)
{
	TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
	while ((TWCR & (1<<TWINT)) == 0);
}

void TWI_stop(void)
{
	TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN);
}

void TWI_write(uint8_t data)
{
	TWDR = data; // Copy data to TWI data register
	TWCR = (1<<TWINT)|(1<<TWEN); // Initialize data transfer
	while ((TWCR & (1<<TWINT)) == 0); // Wait for transmission complete
}

uint8_t TWI_readACK(void)
{
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);
	while ((TWCR & (1<<TWINT)) == 0);
	return TWDR;
}

uint8_t TWI_readNACK(void)
{
	TWCR = (1<<TWINT)|(1<<TWEN);
	while ((TWCR & (1<<TWINT)) == 0);
	return TWDR;
}

uint8_t TWI_status(void)
{
	uint8_t status;
	status = TWSR & 0xF8; // 3 LSB in TWSR are invalid, so we mask them out
	return status;
}

void UART_init (uint16_t BAUD)
{
	//uint16_t BRREGVAL = ((FOSC/16)/BAUD)-1; //Compute BAUD register value from defined CPU clock and desired Baud rate
	UBRR0H = (BRREGVAL>>8); // -----v
	UBRR0L = BRREGVAL; // Set baud rate for UART
	UCSR0A |= (1<<MPCM0); // Set multiprocessor communication and standard rate
	UCSR0B |= (1<<UCSZ02)|(1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0); // -----------------------------------------v
	UCSR0C |= (1<<UCSZ00)|(1<<UCSZ01);//|(1<<UPM00)|(1<<UPM01); // Enable RX and TX, set 9-bit data format (9th bit indicates address/data), one STOP bit, odd? parity, enable parity
}

void UART_transmit_address( uint8_t data )
{
	while ( !( UCSR0A & (1<<UDRE0)) ); // Wait for empty buffer to begin
	UCSR0B |= (1<<TXB80); // 9th bit set indicates address frame
	UDR0 = data; // Put data into buffer and start transmission
}

void UART_transmit_data( uint8_t data )
{
	while ( !( UCSR0A & (1<<UDRE0)) ); // Wait for empty buffer to begin
	UCSR0B &= ~(1<<TXB80); // 9th bit not set indicates data frame
	UDR0 = data; // Put data into buffer and start transmission
}

void UART_flush( void )
{
	uint8_t dummy;
	while ( UCSR0A & (1<<RXC0) ) dummy = UDR0; // Flushes UDR0
}

/*Section with vital UART interrupt routines*/

ISR (USART_RX_vect)
{
	uint8_t status, data, bit;
	while ( !(UCSR0A & (1<<RXC0)) ); // Wait for data reception to complete
	status = UCSR0A;
	bit = UCSR0B;
	data = UDR0;
	if ( status & ((1<<FE0)|(1<<DOR0)|(1<<UPE0)) ) // Check for errors (frame error, data overrun, parity), and eventually return from routine
	{
		return;
	}
	if ((UCSR0A & MPCM0) && (data == UART_ADDRESS)) // See if in multi mode and if device address checks
	{
		UCSR0A &= ~(1<<MPCM0); // Next data received will be always read
		return;
	}	
	else if (!(UCSR0A & MPCM0))
	{
		bit = (bit >> 1) & 0x01; // Get 9th bit of data frame
		if (bit == 0x01) // Check if there is no problem with data format, else return from interrupt
			return;
		UCSR0B &= ~(1<<RXCIE0); // Disable UART RX interrupts (just to be sure)
		/* Start code for sensor measurement */
		UART_transmit_data(data);
	}	
	else
		return;
}

ISR (USART_TX_vect)
{
	UCSR0B |= (1<<RXCIE0); // Enable UART RX interrupts after code execution
}


