/*
Timer0_init: Richtet Timer0 für den TWI_Watchdog ein. Nebenbei wird ein 0.01ms Tick erzeugt, kann man immer mal gebrauchen.

TWI_init:	Richtet das TWI ein

TWI_Read:	Liest Daten aus einen TWI/I2C-Slave aus. Hierzu in den TWI_SendBuffer!!!
			[0] SLA+W, [1]Subadresse(Register vom Slave aus dem gelesen werden soll),
			[2] SLA+W. In TWI_DataSize die Anzahl zu lesender Bytes eintragen.
			Dann TWI_Read() ausführen.

TWI_Send:	Sendet Daten TWI/I2C-Slave. Hierzu den TWI_SendBuffer mit [0] SLA+W
			[1]Subadresse(Register vom Slave, in das geschrieben werden soll) 
			[2]zu schreibende Daten. Standardmäßig ist der SendBuffer 10 Byte groß.
			Dann TWI_Write() ausführen.
			
TWI_WatchdogCheck: Überprüft, ob Fehler aufgetreten sind.
*/
void Timer0_init(void);
void TWI_ClearReadBuffer(void);
void TWI_ClearSendBuffer(void);
void TWI_init(void);
void TWI_Read(void);
void TWI_Send(void);
void TWI_WatchdogCheck(void);

/*
TWI_Flags: bit0 = Send_Flag  bit1 = Recieve_Flag  bit2 = StreamingMode  bit7 TWI_Error_Flag

TWI_SendBuffer:	Hier die zu Senden Daten. TWI_SendBuffer[0] = SAD+W, [1] = Subadresse, [2] = zu sendendes Datum....
				Auch zum Lesen müssen die Adressen hier rein: [0] = SAD+W, [1] = Subadresse, [2] = SAD+R

TWI_ReadBuffer:	Hier landen die zu lesenden Daten. Beide Buffer auf bis zu 2^16 - 1Byte erweiterbar, 
				wenn genügend Platz vorhanden. Liegt an den 16Bit-TWI_Read/Send-Countern

TWI_DataSize:   Hier kommt die Anzahl der zu lesenden/sendenden Bytes rein.
*/


volatile uint8_t TWI_Flags = 0;
volatile uint8_t TWI_ReadBuffer[TWI_ReadBufferSize] = {0};
volatile uint8_t TWI_SendBuffer[TWI_SendBufferSize] = {0};
volatile uint8_t TWI_Watchdog = 0;	


volatile uint16_t Tick_100telMillis;
volatile uint16_t TWI_DataSize = 0;
volatile uint16_t TWI_ReadCnt = 0;
volatile uint16_t TWI_SendCnt = 0;

ISR(TIMER0_COMPA_vect)
{
	if( (TWI_Flags & (1 << TWI_Send_Flag)) | (TWI_Flags & (1 << TWI_Read_Flag)) | (TWI_Flags & (1 << TWI_StreamingMode_Flag)) )
	{
		TWI_Watchdog++;		
		Tick_100telMillis++;
	}else
	{
		Tick_100telMillis++;
	}
	
}


/*
Statemachine nach Atmelappnote AVR315
*/
ISR(TWI_vect)
{
	switch (TWI_Flags)
	{
		case ( (1 << TWI_Send_Flag) ):		
			
			switch (TW_STATUS)
			{
				case TW_START:
					
					TWI_SendCnt = 0;									//Reset Buffer
					TWI_Watchdog = 0;					
					TWDR = TWI_SendBuffer[TWI_SendCnt];					//Copy Data to TWDR
					TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
					TWI_SendCnt++;										//Postincrement "Buffer"
				break;
		
				
				
				case TW_REP_START:
					
					TWI_SendCnt = 0;									//Reset Buffer
					TWI_Watchdog = 0;
					
					if( (TWI_SendCnt == TWI_DataSize) | (TWI_SendCnt >= TWI_SendBufferSize) )						//Buffer am Ende?
					{
						TWI_Flags = 0;
						TWI_SendCnt = 0;
						TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN) | (1 << TWIE);
					}
					else
					{
						TWDR = TWI_SendBuffer[TWI_SendCnt];					//Copy Data to TWDR
						TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
						TWI_SendCnt++;
					}	
				break;
		
				
				
				case TW_MT_SLA_ACK:
				
					TWI_Watchdog = 0;
					
					if( (TWI_SendCnt == TWI_DataSize) | (TWI_SendCnt >= TWI_SendBufferSize) )					//Buffer am Ende?
					{
						TWI_Flags = 0;
						TWI_SendCnt = 0;
						TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN) | (1 << TWIE);
					}
					else
					{
						TWDR = TWI_SendBuffer[TWI_SendCnt];					//Copy Data to TWDR
						TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
						TWI_SendCnt++;
					}	
				break;
				
				
				
				case TW_MT_DATA_ACK:
					
					TWI_Watchdog = 0;
					
					if( (TWI_SendCnt == TWI_DataSize) | (TWI_SendCnt >= TWI_SendBufferSize) )					//Buffer am Ende?
					{
						TWI_Flags = 0;
						TWI_SendCnt = 0;
						TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN) | (1 << TWIE);
					}
					else
					{
						TWDR = TWI_SendBuffer[TWI_SendCnt];					//Copy Data to TWDR
						TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
						TWI_SendCnt++;
					}
					
					
				break;
					
						
			
				case TW_MT_SLA_NACK:
			
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;	
				
				case TW_BUS_ERROR:
				
				TWCR = (1 << TWINT);
				TWI_Flags = 0;
				TWCR = (1 << TWEN) | (1 << TWIE);
				TWI_Watchdog = 0;
				break;		
			}
		break;
		
		case ( (1 << TWI_Read_Flag) ):
		
			switch (TW_STATUS)
			{
				case TW_START:
					
					TWI_SendCnt = 0;									//Reset Buffer
					TWI_Watchdog = 0;					
					TWDR = TWI_SendBuffer[TWI_SendCnt];					//Copy Data to TWDR
					TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
					TWI_SendCnt++;										//Postincrement "Buffer"
				break;
				
				
				
				case TW_REP_START:
					
					TWI_ReadCnt = 0;									//Reset Buffer
					TWI_Watchdog = 0;
					TWDR = TWI_SendBuffer[TWI_SendCnt];					//Hier muss dann SAD+R stehen, also an dritter Stelle im SendBuffer
					TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
					TWI_SendCnt++;	
				break;
				
				
				
				case TW_MT_SLA_ACK:
	
					TWI_Watchdog = 0;
					
					TWDR = TWI_SendBuffer[TWI_SendCnt];			
					TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT
					TWI_SendCnt++;
				break;
					
				
				
				case TW_MT_DATA_ACK:
				
					TWI_Watchdog = 0;
					
					TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE);	//Clear TWINT, RepeatedStart Senden
				break;						
				
				
				
				case TW_MR_ARB_LOST:
				
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;
				
				
				
				case TW_MR_SLA_ACK:
					
					TWI_Watchdog = 0;
				
					TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN) | (1 << TWIE);				
				break;
				
				
				
				case TW_MR_SLA_NACK:
				
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;
				
				
				
				case TW_MR_DATA_ACK:
				
					TWI_Watchdog = 0;
				
					if( (TWI_ReadCnt < TWI_DataSize - 1) | (TWI_ReadCnt >= TWI_ReadBufferSize) )
					{
						TWI_ReadBuffer[TWI_ReadCnt] = TWDR;
						TWI_ReadCnt++;
						TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN) | (1 << TWIE);
					}
					else
					{
						TWI_ReadBuffer[TWI_ReadCnt] = TWDR;
						TWI_Flags = 0;
						TWI_ReadCnt = 0;
						TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN) | (1 << TWIE);	
					}					
				break;
				
				
				
				case TW_MR_DATA_NACK:
				
					TWI_Flags |= (1 << TWI_Error_Flag);
				break;
				
				case TW_BUS_ERROR:
				
					TWCR = (1 << TWINT);
					TWI_Flags = 0;
					TWCR = (1 << TWEN) | (1 << TWIE);
					TWI_Watchdog = 0;
				break;
			}
	}
		
}

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

void TWI_ClearReadBuffer(void)
{
	uint16_t i = 0;
	
	for(i = 0; i < TWI_ReadBufferSize; i++)
	{
		TWI_ReadBuffer[i] = 0;
	}
}

void TWI_ClearSendBuffer(void)
{
	uint16_t i = 0;
	
	for(i = 0; i <= TWI_SendBufferSize; i++)
	{
		TWI_SendBuffer[i] = 0;
	}
}

void TWI_init(void)
{
	TWBR = 12;
	// Load data register with default content; release SDA
	TWDR = 0xff;

	// Enable TWI peripheral with interrupt disabled
	TWCR = (0<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(0<<TWWC)|(1<<TWEN)|(0<<TWIE);
	Timer0_init();
}

void TWI_Read(void)
{	
	TWI_Flags |= (1 << TWI_Read_Flag);
	TWSR = 0;							//Clear Status
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE);
	while ( (TWI_Flags & (1 << TWI_Send_Flag)) | (TWI_Flags & (1 << TWI_Read_Flag)) )
	{
		TWI_WatchdogCheck();
	}
}

void TWI_Send()
{	
	TWI_Flags |= (1 << TWI_Send_Flag);
	TWSR = 0;							//Clear Status
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE);
	while ( (TWI_Flags & (1 << TWI_Send_Flag)) | (TWI_Flags & (1 << TWI_Read_Flag)) )
	{
		TWI_WatchdogCheck();
	}
}

void TWI_WatchdogCheck(void)
{
	uint8_t i = 0;
	
	if( (TWI_Watchdog >= TWI_WatchdogMax) | (TWI_Flags & (1 << TWI_Error_Flag)) )			//Einmal pro Durchlauf prüfen, ob Fehler aufgetreten sind, falls ja, TWI reset.
	   {
		   TWCR = (1 << TWINT);
		   for(i = 0; i <= 200; i++)
		   {
			   
		   }
		   TWI_Flags = 0;
		   TWCR = (1 << TWEN) | (1 << TWIE);
		   TWI_Watchdog = 0;
	   }
}