/*
 * i2c.c
 *
 * Created: 12/7/2021 1:13:35 PM
 *  Author: david
 */ 
#include "main.h"

static uint32_t flexcom_id_lookup(Flexcom* flxcm) {
	if(flxcm == FLEXCOM0) {
		return ID_FLEXCOM0;
		} else if(flxcm == FLEXCOM1){
		return ID_FLEXCOM1;
		} else if(flxcm == FLEXCOM2){
		return ID_FLEXCOM2;
		} else if(flxcm == FLEXCOM3){
		return ID_FLEXCOM3;
		} else if(flxcm == FLEXCOM4){
		return ID_FLEXCOM4;
		} else if(flxcm == FLEXCOM5){
		return ID_FLEXCOM5;
		} else if(flxcm == FLEXCOM6){
		return ID_FLEXCOM6;
#if defined(__SAMG55J19__)
		} else if(flxcm == FLEXCOM7){
		return ID_FLEXCOM7;
#endif
	}
	return ID_PERIPH_COUNT; // Invalid ID
}

static void enable_periph_clk(uint32_t ul_id)
{
	if (ul_id <= MAX_PERIPH_ID) 
	{

		if (ul_id < 32) {
			if ((PMC->PMC_PCSR0 & (1u << ul_id)) != (1u << ul_id)) 
			{
				PMC->PMC_PCER0 = 1 << ul_id;
			}
		} else {
			ul_id -= 32;
			if ((PMC->PMC_PCSR1 & (1u << ul_id)) != (1u << ul_id)) 
			{
				PMC->PMC_PCER1 = 1 << ul_id;
			}
		}
	}
}

static uint32_t calculate_clock_period_register(uint32_t periph_clock, uint32_t desired_i2c_clock) {
	// Setting t_high = t_low = 1/2 t_i2c ... so f_high = f_low = 2*f_i2c
	// (CHDIV * (2^CKDIV) + 3)*t_periph = t_i2c
	// t_i2c / t_periph = (CHDIV * (2^CKDIV) + 3)
	// where f_periph = 1/t_periph, and f_i2c = 1/t_i2c
	// f_periph / f_i2c = (CHDIV * (2^CKDIV) + 3)
	// CHDIV * (2^CKDIV) = f_periph / f_i2c - 3
	// Then... find the smallest CKDIV that allows for the division factor
	// to fit into CHDIV / CLDIV, which are 8 bit numbers

	uint32_t f_pulse = 2*desired_i2c_clock;
	uint32_t chdiv = periph_clock / f_pulse - 3;
	uint8_t ckdiv = 0;
	while(chdiv > 255) {
		chdiv = chdiv >> 1UL;
		ckdiv++;
	}

	return TWI_CWGR_CHDIV((uint8_t)chdiv) + TWI_CWGR_CLDIV((uint8_t)chdiv) + TWI_CWGR_CKDIV(ckdiv);

}

static uint8_t i2c_write_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
static uint8_t i2c_read_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
static uint8_t i2c_write_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
static uint8_t i2c_read_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);

void init_i2c(I2C_Handle* hi2c, uint32_t desired_clock_speed) {
	Flexcom* flxcm = hi2c->flexcom;
	Twi* twi = (Twi*)(((uint32_t)flxcm) + TWI_OFFSET_FROM_FLEXCOM);

	// Sanity check
	uint32_t flexcom_id = flexcom_id_lookup(flxcm);
	if(flexcom_id >= ID_PERIPH_COUNT){
		return;
	}

	// Enable clocks to necessary peripherals
	enable_periph_clk(flexcom_id);
	//sysclk_enable_peripheral_clock(flexcom_id);
	
	// Put the Flexcom into I2C mode
	flxcm->FLEXCOM_MR = FLEXCOM_MR_OPMODE_TWI;
	
	// Perform software reset of TWI module
	twi->TWI_CR = TWI_CR_SWRST;
	
	// Setup for initiator mode
	twi->TWI_CR = TWI_CR_MSEN + TWI_CR_SVDIS;
	
	// TODO: change fixed clock speed to grab from device settings
	twi->TWI_CWGR = calculate_clock_period_register(MAIN_CLOCK_FREQ, desired_clock_speed);
	// For 400kHz on 98.304MHz clock:
	// twi->TWI_CWGR = TWI_CWGR_CHDIV(15) + TWI_CWGR_CLDIV(15) + TWI_CWGR_CKDIV(3);
	
	hi2c->busy = pdFALSE;
	hi2c->status = I2C_IDLE;
	
	// Enable interrupt vector
	NVIC_EnableIRQ(flexcom_id);
	// ID is shared between clock control and IRQ number. So we can use the same ID
	NVIC_SetPriority(flexcom_id, I2C_INTERRUPT_PRIO);
#if defined(__FREERTOS__)
	hi2c->i2c_mutex = xSemaphoreCreateMutex();
#endif
}

#if defined (__FREERTOS__)
uint8_t i2c_lock(I2C_Handle* hi2c) {
	return xSemaphoreTake(hi2c->i2c_mutex, portMAX_DELAY);
}

uint8_t i2c_unlock(I2C_Handle* hi2c) {
	return xSemaphoreGive(hi2c->i2c_mutex);
}

#endif

uint8_t i2c_transact(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	if(hi2c->status == I2C_NOT_INIT) return pdFAIL;
	switch(hi2c_txn->ttype) {
		case I2C_SIMPLE_WRITE:
		return i2c_write_simple(hi2c, hi2c_txn);
		case I2C_SIMPLE_READ:
		return i2c_read_simple(hi2c, hi2c_txn);
		case I2C_INT_ADDR_WRITE:
		return i2c_write_extended(hi2c, hi2c_txn);
		case I2C_INT_ADDR_READ:
		return i2c_read_extended(hi2c, hi2c_txn);
		default:
		return pdFAIL;
	}
}

static uint8_t i2c_write_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);

	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->data[hi2c->sent_bytes++];
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	
	return pdPASS;
}

static uint8_t i2c_read_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);

	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr))
	+ TWI_MMR_MREAD;
	
	// Special case for single byte read. Must set START and STOP simultaneously
	if(hi2c_txn->len == 1) {
		twi->TWI_CR = TWI_CR_START + TWI_CR_STOP;
		} else {
		// multiple byte transfers set the STOP bit just after the 2nd-to-last byte was read
		twi->TWI_CR = TWI_CR_START;
	}
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_RXRDY;
	
	return pdPASS;
}

static uint8_t i2c_write_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	// Two ways to do this:
	// 1) use the SAMG55 TWI module's internal address capabilities
	//		this involves setting the "address size" to non-zero
	//		before starting a transaction. The MCU will send the device
	//		address then the bytes in internal address register IADR
	//		then start transmitting data from the transmit holding reg
	// 2) do it manually. the internal address bytes can simply be sent
	//		before the data as if they were normal data bytes
	// I chose option 2.
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->internal_addr;
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	
	return pdPASS;
}

static uint8_t i2c_read_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	// Again, 2 ways to do this:
	// 1) use SAMG55 TWI's internal address capabilites
	//		set IADRSZ to non-zero, set address in IADR
	//		after setting start bit, MCU will send device
	//		address, the internal address, then generate
	//		a repeated start, send the device address in
	//		read mode, and then finally start reading bytes
	// 2) do it manually. have to send the internal address
	//		as if it's a normal data byte to write, then
	//		send a repeated start by setting START in the
	//		control register CR. Then proceed with a read
	//		transaction
	// I chose option 2
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->internal_addr;
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	// Note this looks a lot like the write command...in fact, the first
	// half of this transaction is pretty much exactly a write with one
	// data byte, the internal address.
	
	
	return pdPASS;
}

uint8_t i2c_busy(I2C_Handle* hi2c) {
	return hi2c->busy;
}

I2C_Status_Type i2c_status(I2C_Handle* hi2c)
{
	return hi2c->status;
}

void i2c_irq_handler(I2C_Handle* hi2c) {
	// Get status to figure out what happened
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	uint32_t status = twi->TWI_SR;

	I2C_Transaction_Request* hi2c_txn = hi2c->current_txn;
	
	// TXRDY - available to push next byte to write
	if((status & TWI_SR_TXRDY) != 0 && (twi->TWI_IMR & TWI_IMR_TXRDY) != 0) {
		// In simple writing mode
		if(hi2c_txn->ttype == I2C_SIMPLE_WRITE || hi2c_txn->ttype == I2C_INT_ADDR_WRITE) {
			if(hi2c->sent_bytes < hi2c_txn->len) {
				// push next byte
				twi->TWI_THR = hi2c_txn->data[hi2c->sent_bytes++];
				} else {
				// final byte was already pushed - set STOP bit to finish transaction
				twi->TWI_CR = TWI_CR_STOP;
				// Now wait for Txcomp
				twi->TWI_IER = TWI_IER_TXCOMP;
				twi->TWI_IDR = TWI_IDR_TXRDY;
			}
		}
		if(hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// If we are here, that means the internal address has been sent to the
			// holding register. We need to trigger the repeated start command
			
			// Change to reading mode first
			twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr))
			+ TWI_MMR_MREAD;
			if(hi2c_txn->len == 1) {
				// If only one byte to transmit, START and STOP should be set simultaneously
				twi->TWI_CR = TWI_CR_START + TWI_CR_STOP;
				} else {
				twi->TWI_CR = TWI_CR_START;
			}
			// And disable the TXRDY interrupt, enable the RXRDY interrupt
			twi->TWI_IDR = TWI_IDR_TXRDY;
			twi->TWI_IER = TWI_IER_RXRDY;
		}
	}
	// RXRDY - responder sent a byte in and it's ready to pull out of the holding register
	if((status & TWI_SR_RXRDY) != 0 && (twi->TWI_IMR & TWI_IMR_RXRDY) != 0) {
		// In simple reading mode
		if(hi2c_txn->ttype == I2C_SIMPLE_READ || hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// pull the byte into our buffer
			hi2c_txn->data[hi2c->sent_bytes++] = twi->TWI_RHR;
			// Get ready to send a nack if this was 2nd-to-last byte
			if(hi2c->sent_bytes == (hi2c_txn->len - 1)) {
				twi->TWI_CR = TWI_CR_STOP;
				// we should get one more RXRDY with the final byte, then the TXCOMP after STOP bit sent
			}
			if(hi2c->sent_bytes == hi2c_txn->len) {
				// Now we are waiting on TXCOMP to finish this transaction
				twi->TWI_IER = TWI_IER_TXCOMP;
				twi->TWI_IDR = TWI_IDR_RXRDY;
			}
		}
	}
	
	// NACK - no reply from responder device
	if((status & TWI_SR_NACK) != 0 && (twi->TWI_IMR & TWI_IMR_NACK) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY + TWI_IDR_RXRDY;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_NO_REPLY;
		if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
	}
	// ARBLST - arbitration lost...some other initiator device on the bus?!
	if((status & TWI_SR_ARBLST) != 0 && (twi->TWI_IMR & TWI_IMR_ARBLST) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY + TWI_IDR_RXRDY;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_ARB_LOST;
		if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
	}
	// TXCOMP - data has been sent, stop has been sent
	if((status & TWI_SR_TXCOMP) != 0 && (twi->TWI_IMR & TWI_IMR_TXCOMP) != 0) {
		// In simple writing mode
		if(hi2c_txn->ttype == I2C_SIMPLE_WRITE || hi2c_txn->ttype == I2C_INT_ADDR_WRITE) {
			// Transaction complete
			twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY;
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
		}
		// In simple reading mode
		if(hi2c_txn->ttype == I2C_SIMPLE_READ || hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// Transaction complete
			twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_RXRDY;
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
		}
	}

}

static void delay_loop_for_lockup_clear(void) {
	// Volatile declaration to prevent compiler optimization
	// from simply eliminating this loop.
	for(volatile uint16_t i = 0; i < 700; i++); // 100us ish delay
}

void i2c_lockup_clear_sequence(I2C_Handle* hi2c) {
	

	Pio* port;
	uint8_t pin;
	if(hi2c->SCL_Pin > 32) { // port B is 32 and up, port A is 0 to 31
		port = PIOB;
		pin = hi2c->SCL_Pin - 32;
		} else {
		port = PIOA;
		pin = hi2c->SCL_Pin;
	}
	// Do a lockup clear sequence on SCL
	
	port->PIO_PER = (1u << pin); // remove peripheral control
	port->PIO_MDER = (1u << pin); // open collector
	port->PIO_OER = (1u << pin); // output
	port->PIO_SODR = (1u << pin); // high level
	
	// Toggle clock pin 10x without caring for data pin
	for(uint8_t i = 0; i < 10; i++) {
		delay_loop_for_lockup_clear();
		port->PIO_CODR = (1u << pin); // toggle low
		delay_loop_for_lockup_clear();
		port->PIO_SODR = (1u << pin); // toggle high
	}
	
	// Release clock pin
	port->PIO_ODR = (1u << pin); // back to input mode
	port->PIO_PDR = (1u << pin); // and peripheral control

}