#include <i2c.h>
#include <util/delay.h>

uint8_t i2c_error;
uint8_t i2c_status;

#define PSC_I2C			1	  // I2C prescaler
#define F_I2C			400000UL  // I2C Clock
#define SET_TWBR		(F_CPU/F_I2C-16UL)/(PSC_I2C*2UL)

void i2c_init(void) {
    i2c_error = 0;
    switch (PSC_I2C) {
    case 4:
        TWSR = 0x01;
        break;
    case 16:
        TWSR = 0x02;
        break;
    case 64:
        TWSR = 0x03;
        break;
    default:
        TWSR = 0x00;
        break;
    }
    TWBR = (uint8_t)SET_TWBR;
    TWCR = (1 << TWEN);
}

void i2c_start(void) {
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
    uint16_t timeout = F_CPU / F_I2C * 2.0;
    while (!(TWCR & (1 << TWINT))) {
        if (--timeout == 0) {
            i2c_error |= (1 << I2C_ERR_START);
            return;
        }
    };
}

void i2c_restart(void) {
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
    uint16_t timeout = F_CPU / F_I2C * 2.0;
    while (!(TWCR & (1 << TWINT))) {
        if (--timeout == 0) {
            i2c_error |= (1 << I2C_ERR_RESTART);
            return;
        }
    };
}

void i2c_write(uint8_t byte) {
    TWDR = byte;
    TWCR = (1 << TWINT) | (1 << TWEN);
    uint16_t timeout = F_CPU / F_I2C * 2.0;
    while (!(TWCR & (1 << TWINT))) {
        if (--timeout == 0) {
            i2c_error |= (1 << I2C_ERR_WRITE);
            return;
        }
    };
}

uint8_t i2c_read_ack(void) {
    TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
    uint16_t timeout = F_CPU / F_I2C * 2.0;
    while ((TWCR & (1 << TWINT)) == 0) {
        if (--timeout == 0) {
            i2c_error |= (1 << I2C_ERR_READACK);
            return 0;
        }
    };
    return TWDR;
}

uint8_t i2c_read_nack(void) {
    TWCR = (1 << TWINT) | (1 << TWEN);
    uint16_t timeout = F_CPU / F_I2C * 2.0;
    while ((TWCR & (1 << TWINT)) == 0) {
        if (--timeout == 0) {
            i2c_error |= (1 << I2C_ERR_READNACK);
            return 0;
        }
    };
    return TWDR;
}

void i2c_stop(void) {
    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
    //while (TWCR & (1 << TWSTO));
}