#include "i2c.h"

#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>
#include <stdint.h>
#include "uart.h" // DEBUG macro

void I2C_init() {
    // Set the prescaler to 1
    TWSR &= ~(1 << TWPS0);
    TWSR &= ~(1 << TWPS1);
    // Set the bit rate to 100kHz
    TWBR = ((F_CPU / 100000) - 16) / 2;
}

uint8_t I2C_start(uint8_t addr) {
    // Send the start condition
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);

    DEBUG("Waiting for start condition to be sent");
    // Wait for the start condition to be sent
    while (!(TWCR & (1 << TWINT)))
        ;

    DEBUG("Start condition sent");
    // Load the address of the slave device
    TWDR = addr;

    DEBUG("Sending address");
    // Clear the TWINT bit to start the transmission
    TWCR = (1 << TWINT) | (1 << TWEN);

    DEBUG("Waiting for address to be sent");
    // Wait for the address to be sent
    while (!(TWCR & (1 << TWINT)))
        ;

    DEBUG("Address sent");
    // Get the status of the transmission
    uint8_t status = TWSR & 0xF8;

    DEBUG("Checking status");
    // Return true if the slave acknowledged the address
    return (status == TW_MT_SLA_ACK || status == TW_MR_SLA_ACK);
}

void I2C_stop() {
    // Send the stop condition
    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
    // Wait for the stop condition to be sent
    while (TWCR & (1 << TWSTO))
        ;
}

uint8_t I2C_write(uint8_t data) {
    // Load the data into the data register
    TWDR = data;
    // Start transmission of data
    TWCR = (1 << TWINT) | (1 << TWEN);
    // Wait for the data to be sent
    while (!(TWCR & (1 << TWINT)))
        ;

    // Return true if the data was sent
    return (TWSR & 0xF8) == TW_MT_DATA_ACK;
}

uint8_t I2C_read(uint8_t ack) {
    // Enable TWI, generate ACK (if ack = 1) and clear TWI interrupt flag
    TWCR = (1 << TWINT) | (1 << TWEN) | (ack << TWEA);
    // Wait until TWI finish its current job (read operation)
    while (!(TWCR & (1 << TWINT)))
        ;
    // Return received data
    return TWDR;
}