#ifndef _I2C_H
#define _I2C_H
#include <avr/interrupt.h>
#define USI_PIN  PINA
#define USI_PORT PORTA
#define USI_DDR  DDRA

#define SDA_PIN (1<<6)
#define SCL_PIN (1<<4)

#define I2C_READ_ADDR(x) ((x)<<1 | 1)
#define I2C_WRITE_ADDR(x) ((x)<<1)

enum {
	I2C_OK = 0, I2C_NAK = 1, I2C_ARB_LOST = 2
};

/** @brief Sets up I2C */
static void i2c_setup(void);
/** @brief Waits for any communication by other masters to finish, then sends a START condition */
static void i2c_send_start(void);
/** @brief Sends a repeated START */
static void i2c_repeat_start(void);
/** @brief Sends a STOP condition */
static void i2c_send_stop(void);
/** @brief Reads a single byte, as master
	@param[in] send_ack Set to 1 to send an ACK, 0 for NAK
	@return The byte that was read
 */
static uint8_t i2c_master_read_byte(uint8_t send_ack);
/** @brief Sends a single byte, as master
	@param[in] v The byte to be written
	@return I2C_OK, I2C_NAK, or I2C_ARB_LOST
 */
static uint8_t i2c_master_send_byte(uint8_t v);

/* Helper functions */
static inline void i2c_pull_scl(void);
static inline void i2c_pull_sda(void);
static inline void i2c_release_scl(void);
static inline void i2c_release_sda(void);
static inline void i2c_wait_scl(void);
static inline void i2c_set_output(uint8_t mask);
static inline void i2c_set_high(uint8_t mask);

volatile uint8_t start_detected = 0; ///< Set to 1 when another master owns the bus

ISR(USI_STR_vect) {
	start_detected = 1;
}

static inline void i2c_pull_scl(void)    { USI_PORT &= ~SCL_PIN; USI_DDR  |= SCL_PIN; }
static inline void i2c_pull_sda(void)    { USI_PORT &= ~SDA_PIN; USI_DDR  |= SDA_PIN; }
static inline void i2c_release_scl(void) { USI_DDR  &= ~SCL_PIN; USI_PORT |= SCL_PIN; }
static inline void i2c_release_sda(void) { USI_DDR  &= ~SDA_PIN; USI_PORT |= SDA_PIN; }
static inline void i2c_release_scl_as_output(void) { USI_PORT |= SCL_PIN; }
static inline void i2c_release_sda_as_output(void) { USI_PORT |= SDA_PIN; }
static inline void i2c_release_scl_as_input(void) { USI_DDR &= ~SCL_PIN; }
static inline void i2c_release_sda_as_input(void) { USI_DDR &= ~SDA_PIN; }
static inline void i2c_wait_scl(void)    { while((USI_PIN & SCL_PIN) == 0); }

static inline void i2c_set_output(uint8_t mask) { USI_DDR |= mask; }
static inline void i2c_set_high(uint8_t mask) { USI_PORT |= mask; }

static void i2c_setup(void) {
	// Enable start condition interrupt, set wire mode to I2C
	// using 2 as wire mode does not hold SCL low on counter overflows
	// using 3 as wire mode does
	USICR = (1<<USISIE) | (2 << USIWM0);

	USIDR = 0xFF;

	i2c_set_high(SCL_PIN|SDA_PIN);
	i2c_set_output(SCL_PIN|SDA_PIN);
	
	// Clear flags
	USISR = (1<<USISIF) | (1<<USIOIF) | (1<<USIPF);
}

static void i2c_send_start(void) {
	if(start_detected == 1) {
		while((USISR & (1<<USIPF)) == 0);
		USISR |= (1<<USIPF);
		start_detected = 0;
	}
	
	i2c_set_high(SCL_PIN|SDA_PIN);
	i2c_set_output(SCL_PIN|SDA_PIN);

	// Clear the counter overflow interrupt flag, to ensure that SCL goes high
	USISR |= (1<<USIOIF);

	i2c_release_scl();
	i2c_wait_scl();
	i2c_pull_sda();
	i2c_pull_scl();
	i2c_release_sda_as_output();
}

static void i2c_repeat_start(void) {
	i2c_release_sda_as_input();
	i2c_release_scl();
	i2c_wait_scl();
	i2c_pull_sda();
	i2c_pull_scl();
	i2c_release_sda_as_output();
}

static void i2c_send_stop(void) {
	i2c_set_output(SCL_PIN | SDA_PIN);
	i2c_pull_sda();
	i2c_release_scl();
	i2c_wait_scl();
	i2c_release_sda();
	
	// Enable start condition interrupt
	USICR |= (1<<USISIE);
}

static uint8_t i2c_master_read_byte(uint8_t send_ack) {
	// Clear flags
	USISR |= (1<<USIDC) | (1<<USIOIF);

	i2c_release_sda_as_input();
	i2c_set_output(SCL_PIN);

	// Set counter to 0
	USISR = 0;
	USIDR = 0;

	do {
		USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC);
		i2c_wait_scl();
		USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC) | (1<<USICLK);
	} while((USISR & (1<<USIOIF)) == 0);

	uint8_t val = USIDR;

	// Now send the ACK
	if(send_ack) i2c_pull_sda();
	USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC);
	i2c_wait_scl();
	USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC);
	i2c_release_sda_as_input();

	return val;
}

static uint8_t i2c_master_send_byte(uint8_t v) {
	// Clear flags
	USISR |= (1<<USIDC) | (1<<USIOIF);

	i2c_set_output(SCL_PIN|SDA_PIN);

	// Set counter to 0, to do 16 clock cycles
	USISR = 0;

	USIDR = v;
	// write out the USI data register
	do {
		USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC);
		i2c_wait_scl();
		USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC) | (1<<USICLK);
	} while((USISR & ((1<<USIOIF)|(0<<USIDC))) == 0);
	// Now SDA is our lowest bit and SCL is low.

	if((USISR & (1<<USIDC)) != 0) {
		goto lost_arbitration;
	}

	// Next, we have to read the ACK bit.
	// The slave pulls SDA low to acknowledge
	USISR = (1<<USIOIF) | 14;
	i2c_release_sda();
	USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC);
	i2c_wait_scl();
	// Clock the ACK bit into USIDR & 1
	USICR = (2<<USIWM0) | (1<<USICS1) | (1<<USITC) | (1<<USICLK);
	i2c_set_output(SDA_PIN);

	// Enable start condition interrupt
	USICR |= (1<<USISIE);

	if(USIDR & 1) {
		return I2C_NAK;
	} else {
		return I2C_OK;
	}
		

 lost_arbitration:
	// Enable start condition interrupt
	USICR |= (1<<USISIE);

	start_detected = 1;
	return I2C_ARB_LOST;
}
#endif