/*
 * X_NUCLEO_IKS01A1.c
 *
 * Created: 20.10.2015 02:14:23
 *  Author: Ichich
 */ 

#define TWI_Send_Flag 0
#define TWI_Write_Flag 1
#define TWI_StreamingMode_Flag 2
#define TWI_Error_Flag 8
//I2C-Adresse,bzw TWI-Adresse
#define Mems3D_Addr 0b1101011

//Registerdefinitionen
#define ACT_THS 0x04

#define ACT_DUR 0x05

#define INT_GEN_CFG_XL 0x06

#define WHO_AM_I 0x0F

#define CTRL_REG1_G 0x10
	#define ODR_G2 7
	#define ODR_G1 6
	#define ODR_G0 5
	#define FS_G1 4
	#define FS_G0 3
	#define BW_G1 1
	#define BW_G0 0

#define CTRL_REG6_XL 0x20
	#define ODR_XL2 7
	#define ODR_XL1 6
	#define ODR_XL0 5
	#define FS1_XL 4
	#define FS0_XL 3
	#define BW_SCAL_ODR 2
	#define BW_XL1 1
	#define BW_XL0 0

#define XGyro_RegHi 0x18
#define XGyro_RegLo 0x19

#define YGyro_RegHi 0x1A
#define YGyro_RegLo 0x1B

#define ZGyro_RegHi 0x1C
#define ZGyro_RegLo 0x1D

#define XAcc_RegHi	0x1E
#define XAcc_RegLo	0x1F

#define YAcc_RegHi  0x20
#define YAcc_RegLo	0x21

#define ZAcc_RegHi	0x22
#define ZAcc_RegLo	0x23


#include <avr/io.h>
#include <interrupt.h>
#include <stdlib.h>
#include <util/twi.h>

void TWI_init(void);
void TWI_Send(void);
void Timer0_init(void);

uint8_t TWI_DataSize = 0;
uint8_t TWI_Watchdog = 0;
uint8_t TWI_Flags = 0; //bit0 = Send_Flag  bit1 = Recieve_Flag  bit2 = StreamingMode  bit8 TWI_Error_Flag  
uint8_t TWI_SendBuffer[10] = {0};
uint8_t TWI_RecBuffer[40] = {0};
	
uint16_t TWI_SendCnt = 0;
uint16_t DataSize = 0;

ISR(TIMER0_COMPA_vect)
{
	if( (TWI_Flags & (1 << TWI_Send_Flag)) | (TWI_Flags & (1 << TWI_Write_Flag)) | (TWI_Flags & (1 << TWI_StreamingMode_Flag)) )
	{
		TWI_Watchdog++;
		PORTE ^= 0xFF;
	}
	
}

ISR(TWI_vect)
{
	switch (TWI_Flags)
	{
		case TWI_Send_Flag:
		
			if(TWI_SendCnt == TWI_DataSize)
			{
				TWI_Flags = 0;
				TWI_SendCnt = 0;
				TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
			}
			switch (TW_STATUS)
			{
				case TW_START:
				
					TWI_Watchdog = 0;
					TWDR = TWI_SendBuffer[TWI_SendCnt];
					TWCR = (1 << TWINT) | (1 << TWEN);
					TWI_SendCnt++;
				break;
		
				case TW_REP_START:
		
					TWI_Watchdog = 0;
					TWDR = TWI_SendBuffer[TWI_SendCnt];
					TWCR = (1 << TWINT) | (1 << TWEN);
					TWI_SendCnt++;
				break;
		
				case TW_MT_SLA_ACK:
		
					TWI_Watchdog = 0;
					TWDR = TWI_SendBuffer[TWI_SendCnt];
					TWCR = (1 << TWINT) | (1 << TWEN);
					TWI_SendCnt++;
				break;
			
				case TW_MT_SLA_NACK:
			
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;
			
				case TW_MR_DATA_ACK:
					
					TWI_Watchdog = 0;
					TWDR = TWI_SendBuffer[TWI_SendCnt];
					TWCR = (1 << TWINT) | (1 << TWEN);
					TWI_SendCnt++;
				break;
			
				case TW_MR_DATA_NACK:
					
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;
			}
			break;
	}
		
}


int main(void)
{	
	uint8_t i = 0;
	
	Timer0_init();
	TWI_init();
	sei();
	
	DDRE = 0b00000011;
	PORTE = 0b00000011;
	TWI_SendBuffer[0] = (Mems3D_Addr << 1) | TW_WRITE;
	TWI_SendBuffer[1] = CTRL_REG1_G;
	TWI_SendBuffer[2] = 0b11000000;
	TWI_DataSize = 3;
	
	TWI_Send();
	
	
	
    while(1)
    {
       TWI_Send(); 
	   
	   if( (TWI_Watchdog > 100) | (TWI_Flags & (1 << TWI_Error_Flag)) )			//Einmal pro Durchlauf prüfen, ob Fehler aufgetreten sind, falls ja, TWI reset.
	   {
		   TWCR = 0;
		   for(i = 0; i <= 200; i++)
		   {
			   
		   }
		   TWI_Flags = 0;
		   TWCR = (1 << TWEN) | (1 << TWIE);
	   }
    }
}


void TWI_init(void)
{
	TWBR = 128;
	TWCR = (1 << TWEN) | (1 << TWIE);
}

void TWI_Send()
{
	while (TWI_Flags & (1 << TWI_Send_Flag))
	{
		
	}
	TWI_Flags |= (1 << TWI_Send_Flag);
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
	
	
	/*
	
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
	
	while ( !( TWCR & (1<<TWINT) ) )					//Wait for TWINT Flag set. This	indicates that the START condition has been transmitted
	{
		
	}
	
	if( (TWSR & 0xF8) != TW_START)				//Fehlerbehandlung
	{
		Errorflag = 1;
		goto Error;
	}
	
	while(i < DataSize)
	{
		TWDR = *(DataPnt + i);
		TWCR = (1 << TWINT) | (1 << TWEN);
	
		while ( !( TWCR & (1<<TWINT) ) )					//Wait for TWINT Flag set. This	indicates that the START condition has been transmitted
		{
		
		}
	
		if(  ( (TWSR & 0xF8) != TW_MT_SLA_ACK ) & ( (TWSR & 0xF8) != TW_MT_DATA_ACK ) & ( (TWSR & 0xF8) != TW_REP_START ) & ( (TWSR & 0xF8) != TW_START )   )   				//Fehlerbehandlung
		{
			Errorflag = 1;
			goto Error;
		}
		i++;
	}
	
	/*TWDR = *(DataPnt + 1);
	TWCR = (1 << TWINT) | (1 << TWEN);
	
	while ( !( TWCR & (1<<TWINT) ) )					//Wait for TWINT Flag set. This	indicates that the START condition has been transmitted
	{
		
	}
	
	if( (TWSR & 0xF8) != TW_MT_DATA_ACK)				//Fehlerbehandlung
	{
		Errorflag = 3;
		goto Error;
	}
	
	
	
	TWDR = *(DataPnt + 2);
	TWCR = (1 << TWINT) | (1 << TWEN);
	
	while ( !( TWCR & (1<<TWINT) ) )					//Wait for TWINT Flag set. This	indicates that the START condition has been transmitted
	{
		
	}
	
	if( (TWSR & 0xF8) != TW_MT_DATA_ACK)				//Fehlerbehandlung
	{
		Errorflag = 3;
		goto Error;
	}*/
	
/*	TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
	
	Error:
	if(Errorflag != 0)
	{
		TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
		TWCR &= ~(1 << TWEN);
		TWCR = (1 << TWEN);
		Errorflag = 0;
	}*/
}

void Timer0_init(void)
{
	TCCR0A = (1 << WGM01);
	TCCR0B = (1 << CS01);
	TIMSK0 = (1 << OCIE0A);
	OCR0A = 200;  //0,1ms-Tick für TWI_Watchdog
}