#include <stdint.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <util/twi.h>
#include <util/delay.h>

#include "i2c_simple.h"
#include "debug.h"

//#define I2C_WAIT() while(!(TWCR & _BV(TWINT)))
#define I2C_WAIT() i2c_wait();


void inline i2c_wait(){
	uint16_t cntr=0;
	while(!(TWCR & _BV(TWINT))){
		cntr++;
		if(cntr >= 1000){
			dbgLog("i2c waittime exceeded!\n");
		}
	}
}



void i2c_init()
{
	TWBR = 32;//bit rate: 100kbit
	TWSR = 0;//Prescaler
	
//	TWAR = 0x80;//our address 1000 000, don't listen to general call
//	TWAMR = 0;
	
	TWCR = _BV(TWEA) | _BV(TWEN);/* _BV(TWINT) clear -> we must do something; unset _BV(TWSTA) manually!, _BV(TWSTO) */
	//TWDR - data
}

uint8_t i2c_read(uint8_t addr, uint8_t len, uint8_t *data)
{
	TWCR |= _BV(TWINT) | _BV(TWSTA);
	I2C_WAIT();
	/* no error possible */
	
	TWDR = addr | 1;
	TWCR &= ~(_BV(TWSTA));
	TWCR |= _BV(TWINT);
	I2C_WAIT();
	/* possible results: ACK, NAK, abitration lost */
	if(TW_STATUS == TW_MR_SLA_NACK)
	{
		TWCR |= _BV(TWINT) | _BV(TWSTO);
		return 0;
	}
	else if(TW_STATUS == TW_MR_ARB_LOST) {return 0;}
	
	uint8_t done = 0;
	while(done + 1 < len)
	{
		TWCR |= _BV(TWEA) | _BV(TWINT);/* send ack after byte */
		I2C_WAIT();
		data[done] = TWDR;
		done++;
		TWCR |= _BV(TWINT);
	}
	
	TWCR &= ~_BV(TWEA);
	TWCR |= _BV(TWINT);/* send nak after byte */
	I2C_WAIT();
	data[done] = TWDR;
	done++;
	TWCR |= _BV(TWINT) | _BV(TWSTO);
	_delay_us(20); // wait about 2 bit periods
	return done;
}

uint8_t i2c_write_i(uint8_t addr, uint8_t len, uint8_t *data, uint8_t stop)
{
	TWCR |= _BV(TWINT) | _BV(TWSTA);
	I2C_WAIT();
	/* no error possible */
	
	TWDR = addr;
	TWCR &= ~(_BV(TWSTA));
	TWCR |= _BV(TWINT);
	I2C_WAIT();
	/* possible results: ACK, NAK, abitration lost */
	if(TW_STATUS == TW_MT_SLA_NACK)
	{
		TWCR |= _BV(TWINT) | _BV(TWSTO);
		return 0;
	}
	else if(TW_STATUS == TW_MT_ARB_LOST) {return 0;}
	
	uint8_t done = 0;
	while(done < len)
	{
		TWDR = data[done];
		TWCR |= _BV(TWINT);
		I2C_WAIT();
		/* possible results: ACK, NAK, abitration lost */
		if(TW_STATUS == TW_MT_DATA_NACK)
		{
			TWCR |= _BV(TWINT) | _BV(TWSTO);
			return done;
		}
		else if(TW_STATUS == TW_MT_ARB_LOST) {return done;}
		done++;
	}
	
	if(stop) {TWCR |= _BV(TWINT) | _BV(TWSTO);}
	_delay_us(20); // wait about 2 bit periods
	return done;
}

inline uint8_t i2c_write(uint8_t addr, uint8_t len, uint8_t *data)
{
	return i2c_write_i(addr, len, data, 1);
}



uint8_t i2c_write_read(uint8_t addr, uint8_t writelen, uint8_t *writedata, uint8_t readlen, uint8_t *readdata)
{
	if(writelen == i2c_write_i(addr, writelen, writedata, 0)) {
		return i2c_read(addr, readlen, readdata);
	}

	return 0;
}