#include "mystd.h"

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

#include "i2c.h"

void i2c_init(void) {
	// clear the status registers
	TWSR = 0x00;
	// set the i2c clock speed
	TWBR = ((F_CPU / I2C_CLOCK) - 16) / 2;
}

void i2c_start(void) {
	//     CLR INT    | SET MASTER |  ENABLE  ;
	TWCR = BV(TWINT) | BV(TWSTA) | BV(TWEN);

	// wait until done
	while (!(TWCR & BV(TWINT)))
		;
}

void i2c_stop(void) {
	//     CLR INT    | ENABLE    |  STOP     ;
	TWCR = BV(TWINT) | BV(TWEN) | BV(TWSTO);
}

void i2c_write(uint8_t data) {
	// set data to write
	TWDR = data;
	//     CLR INT    | ENABLE   ;
	TWCR = BV(TWINT) | BV(TWEN);

	// wait until done
	while (!(TWCR & BV(TWINT)))
		;
}

uint8_t i2c_read_ack(void) {
	TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA);
	while (!(TWCR & BV(TWINT)))
		;
	return TWDR;
}

uint8_t i2c_read_nack(void) {
	TWCR = BV(TWINT) | BV(TWEN);
	while (!(TWCR & (1 << TWINT)))
		;
	return TWDR;
}
void pca9555_write(uint8_t addr, uint8_t reg, uint8_t value) {
	i2c_start();
	i2c_write((addr << 1) | TW_WRITE);	// write mode
	i2c_write(reg);
	i2c_write(value);
	i2c_stop();
}

uint8_t pca9555_read(uint8_t addr, uint8_t reg) {
	uint8_t val;

	i2c_start();
	i2c_write((addr << 1) | TW_WRITE);
	i2c_write(reg);

	i2c_start();  // repeated start
	i2c_write((addr << 1) | TW_READ);
	val = i2c_read_nack();

	i2c_stop();
	return val;
}