#include "i2c.h"
#include "json.h"

void I2C_init(void)
{
	printf("Initializing I2C...\r\n");
	GPIO_InitTypeDef GPIO_InitStruct;
	I2C_InitTypeDef I2C_InitStruct;

	// enable APB1 peripheral clock for I2C1
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);

	GPIO_StructInit(&GPIO_InitStruct);
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; // PB6 and PB7
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_OD;			// set pins to alternate function
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;		// set GPIO speed
	GPIO_Init(GPIOB, &GPIO_InitStruct);					// init GPIOB
	
	// configure I2C1
	I2C_StructInit(&I2C_InitStruct);
	I2C_InitStruct.I2C_ClockSpeed = 100000; 		// 100kHz(standard) vs 400
	I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;			// I2C mode
	I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;	// 50% duty cycle --> standard
	I2C_InitStruct.I2C_OwnAddress1 = 0x00;			// own address, not relevant in master mode
	I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;		// disable acknowledge when reading (can be changed later on)
	I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // set address length to 7 bit addresses
	I2C_Init(I2C1, &I2C_InitStruct);				// init I2C1

	//Enable clock stretching
	I2C_StretchClockCmd(I2C1, ENABLE);

	// enable I2C1
	I2C_Cmd(I2C1, ENABLE); //sets PE bit in CR1, at end`
}

void I2C_start(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction)
{
	printf("Sending I2C Start...\r\n");
	while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY))
	{
		// wait until I2C1 is not busy any more
	}
	printf("I2C ready\r\n");
	I2C_GenerateSTART(I2Cx, ENABLE);
	printf("Start sent\r\n");
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
	{
		// wait for I2C1 EV5 --> Slave has acknowledged start condition
	}
	printf("Start ACK\r\n");
	I2C_Send7bitAddress(I2Cx, address, direction);
	printf("Master -> Slave Address and R/W sent\r\n");
	if(direction == I2C_Direction_Transmitter)
	{
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
		{
			// wait for I2Cx EV6, check if Slave has acknowledged Master transmitter mode
		}
		printf("Slave -> Master ACK\r\n");
	}
	else if(direction == I2C_Direction_Receiver)
	{
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))
		{
			// wait for I2Cx EV6, check if Slave has acknowledged Master receiver mode
		}
		printf("Slave -> Master ACK\r\n");
	}
}

void I2C_restart(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction)
{
	printf("Sending repeated I2C Start...\r\n");
	I2C_GenerateSTART(I2Cx, ENABLE);
	printf("Start sent\r\n");
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
	{
		// wait for I2C1 EV5 --> Slave has acknowledged start condition
	}
	printf("Start ACK\r\n");
	I2C_Send7bitAddress(I2Cx, address, direction);
	printf("Master -> Slave Address and R/W sent\r\n");
	if(direction == I2C_Direction_Transmitter)
	{
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
		{
			// wait for I2Cx EV6, check if Slave has acknowledged Master transmitter mode
		}
		printf("Slave -> Master ACK\r\n");
	}
	else if(direction == I2C_Direction_Receiver)
	{
		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))
		{
			// wait for I2Cx EV6, check if Slave has acknowledged Master receiver mode
		}
		printf("Slave -> Master ACK\r\n");
	}
}

void I2C_write(I2C_TypeDef* I2Cx, uint8_t data)
{
	printf("Sending I2C byte %02x...\r\n",data);
	I2C_SendData(I2Cx, data);
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
	{
		// wait for I2C1 EV8_2 --> last byte is transmitted
	}
	printf("Data sent\r\n");
}

uint8_t I2C_read_ack(I2C_TypeDef* I2Cx)
{
	printf("Listening for byte in I2C (ACK)\r\n");
	I2C_AcknowledgeConfig(I2Cx, ENABLE);
	while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED))
	{
		// wait until one byte has been received
	}
	uint8_t data = I2C_ReceiveData(I2Cx);
	printf("Received byte: %02x\r\n",data);
	return data;
}

uint8_t I2C_read_nack(I2C_TypeDef* I2Cx)
{
	printf("Listening for byte in I2C (NACK)\r\n");
	I2C_AcknowledgeConfig(I2Cx, DISABLE);
	//I2C_GenerateSTOP(I2Cx, ENABLE);
	while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED))
	{
		// wait until one byte has been received
	}
	uint8_t data = I2C_ReceiveData(I2Cx);
	printf("Received byte: %02x\r\n",data);
	return data;
}

void I2C_stop(I2C_TypeDef* I2Cx)
{
	printf("Sending I2C Stop...\r\n");
	I2C_GenerateSTOP(I2Cx, ENABLE);
	/*while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTING))
	{
		// wait for I2C1 EV8_2 --> byte has been transmitted
	}*/
	printf("Stop sent\r\n");
}

uint8_t I2C_check(I2C_TypeDef* I2Cx, uint8_t address)
{
	while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY))
	{
		// wait until I2C1 is not busy any more
	}
	I2C_GenerateSTART(I2Cx, ENABLE);
	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
	{
		// wait for I2C1 EV5 --> Slave has acknowledged start condition
	}
	printf("Probing address %x ...\r\n",address);
	I2C_Send7bitAddress(I2Cx, address, I2C_Direction_Transmitter);
	chThdSleepMilliseconds(I2C_TIMEOUT);
	if(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
	{
		return 1;
	}
	printf("I2C address detected: %x\r\n", address);
	I2C_write(I2Cx,0x00);
	I2C_stop(I2Cx);
	return 0;
}

void I2C_reset(I2C_TypeDef* I2Cx)
{
	printf("Resetting I2C...\r\n");
	I2C_SoftwareResetCmd(I2Cx, ENABLE);
	I2C_DeInit(I2Cx);
	I2C_init();

}

void I2C_scan(I2C_TypeDef* I2Cx,uint8_t *addresses)
{
	uint8_t i, j = 0;
	I2C_init();
	for(i = 0; i < TOTAL_SENSORS; i++)
	{
		if(!I2C_check(I2Cx,sensors[i]->ID << 1))
		{
			addresses[j++] = sensors[i]->ID;
			register_sensor(*sensors[i]);
		}
		else
		{
			I2C_reset(I2Cx);
		}
	}
	register_sensor(battery);
}