#include <stdlib.h>
#include "cpu.h"
#include "mcf5xxx.h"
#include "m52xxevb.h"
#include "i2c.h"
#include "debug.h"

//#define I2C_DEBUG


static unsigned int I2C_DIV_TABLE[]={
	  20,  22,  24,  26,  28,  28,  30,  32,  34,  36,  40,  40,  44,  48,  48,
	  56,  56,  64,  68,  72,  80,  80,  88,  96, 104, 112, 128, 128, 144, 160,
	 160, 192, 192, 224, 240, 256, 288, 320, 320, 384, 384, 448, 480, 512, 576,
	 640, 640, 768, 768, 896, 960,1024,1152,1280,1280,1536,1536,1792,1920,2046,
	2304,2560,3072,3840
};

static uint8 I2C_IC_TABLE[]={
	32,33,34,35,36, 0, 1,37, 2,38, 3,39, 4,40, 5, 6,41,42, 7,43, 8,44, 9,45,10,
	46,11,47,12,13,48,49,14,50,15,51,16,17,52,18,53,54,19,55,20,56,21,57,22,58,
	23,59,24,60,25,61,26,62,27,63,28,29,30,31
};

//#define SYSTEM_CLOCK	75
#define I2C_CLOCK		100		//Clock of i2c bus in kHz
#define I2C_BusMaxWait	1000 	//How long are we willing to wait for I2C bus to become free

int	I2C_isInit=0;

/**
 * Selects the closest DIV to match frequence given by freqKhz.
 * Refer to I2C chapter "I2C Frequency Divider Register (I2FDR)"
 **/
uint8 I2C_divSelect(int freqKhz){
	int approxDiv=(SYSTEM_CLOCK*1000000)/(freqKhz*1000);
	int i;
	for(i=0; i<64 && I2C_DIV_TABLE[i]<approxDiv; i++);
	if(i==0) return I2C_IC_TABLE[i];
	return ((approxDiv-I2C_DIV_TABLE[i-1]) < (I2C_DIV_TABLE[i]-approxDiv)) ? I2C_IC_TABLE[i-1] : I2C_IC_TABLE[i];
}

#ifdef I2C_DEBUG
/**
 * Prints values of all MCF523x registers which affect behaviour of I2C
 **/
void I2C_printRegs(){
	DBG_PRINTF("PAR_FECI2C: %02x\n", MCF_GPIO_PAR_FECI2C);
	DBG_PRINTF("PODR_FECI2C: %02x\n", MCF_GPIO_PODR_FECI2C);
	DBG_PRINTF("PDDR_FECI2C: %02x\n", MCF_GPIO_PDDR_FECI2C);
	DBG_PRINTF("PPDSDR_FECI2C: %02x\n", MCF_GPIO_PPDSDR_FECI2C);
	DBG_PRINTF("PCLRR_FECI2C: %02x\n", MCF_GPIO_PCLRR_FECI2C);
	DBG_PRINTF("DSCR_FECI2C: %02x\n", MCF_GPIO_DSCR_FECI2C);
	DBG_PRINTF("I2C_I2AR: %02x\n", MCF_I2C_I2AR);
	DBG_PRINTF("I2C_I2FDR: %02x\n", MCF_I2C_I2FDR);
	DBG_PRINTF("I2C_I2CR: %02x\n", MCF_I2C_I2CR);
	DBG_PRINTF("I2C_I2SR: %02x\n", MCF_I2C_I2SR);
	DBG_PRINTF("I2C_I2DR: %02x\n", MCF_I2C_I2DR);
	DBG_PRINTF("I2C_I2ICR: %02x\n", MCF_I2C_I2ICR);
}
#endif

/**
 * Prepares I2C bus.
 **/
void I2C_init(){
	int i;

	//Configure the Pin Assignment register
#if defined(MCF_GPIO_PAR_FECI2C)
	MCF_GPIO_PAR_FECI2C |= 0x0F;
#elif defined(MCF5282_GPIO_PASPAR)
	MCF5282_GPIO_PASPAR |= 0x000F; //Yes, 16bit register
#else
	#warning PIN ASSIGNMENT REGISTER IS NOT CONFIGURED
#endif
	//I2C_printRegs();

	//Set clock
	MCF_I2C_I2FDR = MCF_I2C_I2FDR_IC(I2C_divSelect(I2C_CLOCK));

	//Enable I2C bus
	MCF_I2C_I2CR = MCF_I2C_I2CR_IEN;

	//if(MCF_I2C_I2SR & MCF_I2C_I2SR_IBB){
	i=I2C_BusMaxWait;
	while( --i && (MCF_I2C_I2SR & MCF_I2C_I2SR_IBB)){
		DBG_PRINTL("I2C_init: Cold reset invoked\n");
		I2C_coldReset();
	}

	I2C_isInit=1;
}


/**
 * Issues a STOP command to the slave device, placing it in the idle state
 * as if it were just power-cycled on
 * See NOTE on MCF5235 Ref Man (Rev 2) page 27-13
 * NOTE I2CR and I2SR are reset
 **/
void I2C_coldReset(){
	uint8 buf;

	MCF_I2C_I2CR = 0x00;
	MCF_I2C_I2CR = 0xA0;
	buf = MCF_I2C_I2DR;	//Dummy read of I2DR
	MCF_I2C_I2SR = 0x00;
	MCF_I2C_I2CR = 0x00;

	//I2CR is reset, re-enable
	MCF_I2C_I2CR = MCF_I2C_I2CR_IEN;
}

/**
 * Wait for bus to be ready and sends an I2C start signal
 * Returns 0 on error 1 on sucess
 **/
int I2C_start(){
	int i;

	if(!I2C_isInit){
		DBG_PRINTL("I2C_start: I2C not initialized\n");
		return 0;
	}
	
	//Wait for bus
	i=I2C_BusMaxWait;
	while (--i && (MCF_I2C_I2SR & MCF_I2C_I2SR_IBB));
	if(!i){
		DBG_PRINTL("I2C_start: wait exhausted\n");
#ifdef I2C_DEBUG
		I2C_printRegs();
#endif
		return 0;
	}
	
	
	//TXAK is disabled so that we dont ack stuff when we dont expect it. Enabled only during receive
	MCF_I2C_I2CR |= (MCF_I2C_I2CR_MSTA | MCF_I2C_I2CR_MTX | MCF_I2C_I2CR_TXAK);	

	return 1;
}

/**
 * Monitors IIF (I2C Interrupt Flag).
 * Returns 1 as soon as it reads 1, and 0 when wait has been exhausted
 **/
int I2C_wait4Slave(){
	int i;

	//Wait for IIF flag to be set.. lets us know data has been sent/received
	i=I2C_BusMaxWait;
	while (--i && !(MCF_I2C_I2SR & MCF_I2C_I2SR_IIF));
	if(!i){
		DBG_PRINTL("I2C_wait4Slave: wait exhausted\n");
#ifdef I2C_DEBUG
		I2C_printRegs();
#endif
		I2C_stop();
		return 0;
	}

	//Clear IFF
	MCF_I2C_I2SR &= ~MCF_I2C_I2SR_IIF;

	return 1;
}

/**
 * Write one byte to the I2C bus
 * returns 0 on faliure 1 on sucess.
 **/
int I2C_putByte(uint8 byte, int isAddr){
	//DBG_PRINTL("I2C_putByte: invoked\n");

	//Send Byte
	MCF_I2C_I2DR=byte;

	if(isAddr){
		int i;
		//Wait for bus
		i=I2C_BusMaxWait;
		while (--i && !(MCF_I2C_I2SR & MCF_I2C_I2SR_IBB));
		if(!i){
			DBG_PRINTL("I2C_putByte: wait exhausted\n");
#ifdef I2C_DEBUG
			I2C_printRegs();
#endif
			return 0;
		}
		//DBG_PRINTF("I2C_putByte: isAddr Routine loops: %d\n", i);
	}

	if(!I2C_wait4Slave()){
		DBG_PRINTL("I2C_putByte: Write failed\n");
#ifdef I2C_DEBUG
		I2C_printRegs();
#endif
		return 0;
	}

	/**
	//Is transfer complete?
	if(!(MCF_I2C_I2SR & MCF_I2C_I2SR_ICF)){
		DBG_PRINTL("I2C_putByte: Transfer failed\n");
		I2C_stop();
		return 0;
	}
	**/
	int i;
	i=I2C_BusMaxWait;
	while (--i && !(MCF_I2C_I2SR & MCF_I2C_I2SR_ICF));
	if(!i){
		DBG_PRINTL("I2C_putByte: Transfer failed\n");
#ifdef I2C_DEBUG
		I2C_printRegs();
#endif
		I2C_stop();
		return 0;
	}

	/*
	//Did we get an ack?
	if(MCF_I2C_I2SR & MCF_I2C_I2SR_RXAK){
		DBG_PRINTL("I2C_putByte: no ack\n");
		I2C_printRegs();
		I2C_stop();
		return 0;
	}
	*/
	i=I2C_BusMaxWait;
	while (--i && (MCF_I2C_I2SR & MCF_I2C_I2SR_RXAK));
	if(!i){
		DBG_PRINTL("I2C_putByte: no ack\n");
#ifdef I2C_DEBUG
		I2C_printRegs();
#endif
		I2C_stop();
		return 0;
	}

	return 1;
}

/**
 * Sends an I2C stop signal.
 **/
void I2C_stop(){
	//DBG_PRINTL("I2C_STOP: stop sent\n");
	MCF_I2C_I2CR &= ~MCF_I2C_I2CR_MSTA;
}

/**
 * Writes len bytes of buf to I2C bus. Data addressed to slave with addr
 * Returns 1 on sucess <=0 faliure
 **/
//TODO: fix return, number of bytes
int I2C_write(uint8 addr, uint8 *buf, int len){
	//DBG_PRINTF("I2C_write: invoked %i\n", len);

	int i=0;

	if(!I2C_isInit){
		DBG_PRINTL("I2C_write: I2C not initialized\n");
		return 0;
	}

	if(len<=0){
		DBG_PRINTL("I2C_write: invoked with invalid length\n");
		return 0;
	}

	if(buf==NULL){
		DBG_PRINTL("I2C_write: invoked with invalid buffer\n");
		return 0;
	}

	if(!I2C_start()){
		DBG_PRINTL("I2C_write: Failed to start.\n");
		return 0;
	}

	//i2c slave add is only 7 bit long, last bit is rx/tx bit
	if(!I2C_putByte( (addr<<1) & 0xfe, 1)){
		DBG_PRINTL("I2C_write: Failed sending address.\n");
		I2C_stop();
		return 0;
	}

	for(i=0; i<len; i++){
		if(!I2C_putByte(*buf++, 0)){
			DBG_PRINTL("I2C_write: Failed sending data.\n");
			I2C_stop();
			return 0;
		}
	}

	I2C_stop();
	return i;
}

/**
 * Writes a bytes of buf to I2C bus. Data addressed to slave with addr
 * Returns 1 on sucess <=0 faliure
 **/
int I2C_writeByte(uint8 addr, uint8 buf){
	//DBG_PRINTL("I2C_writeByte: invoked\n");
	return I2C_write(addr, &buf, 1);
}

//TODO: fix return, number of bytes
int I2C_read(uint8 addr, uint8 *buf, int len){
	//DBG_PRINTF("I2C_read: invoked %i\n", len);
	int i=0;


	if(!I2C_isInit){
		DBG_PRINTL("I2C_read: I2C not initialized\n");
		return 0;
	}

	if(len<=0){
		DBG_PRINTL("I2C_read: invoked with invalid length\n");
		return -1;
	}

	if(buf==NULL){
		DBG_PRINTL("I2C_read: invoked with invalid buffer\n");
		return -1;
	}

	if(!I2C_start()){
		DBG_PRINTL("I2C_read: Failed to start.\n");
		return -1;
	}

	//i2c slave add is only 7 bit long, last bit is rx/tx bit
	if(!I2C_putByte( (addr<<1) | 0x01 , 1)){
		DBG_PRINTL("I2C_read: Failed sending address.\n");
		I2C_stop();
		return -1;
	}

	//Enable reading and acking
	MCF_I2C_I2CR &= ~MCF_I2C_I2CR_MTX;
	if(len>1){
		MCF_I2C_I2CR &= ~MCF_I2C_I2CR_TXAK;
	}

	//First byte is all 0 and is garbage
	i=(int) MCF_I2C_I2DR;

	for(i=0; i < len; i++){
		if(!I2C_wait4Slave()){
			DBG_PRINTL("I2C_read: timeout\n");
			I2C_stop();
			return 0; 
		}

		if(i==len-2){
			//Disable ack on second to last send
			MCF_I2C_I2CR |= MCF_I2C_I2CR_TXAK;
		}else if(i==len-1){
			//Stop signal on last byte
			I2C_stop();
		}
			
		//Save the byte
		*buf++ = MCF_I2C_I2DR;
		//DBG_PRINTF("I2C_read: %02x\n", *buf);
		//buf++;
	}

	return len;
}