#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(uint32_t SCL_CLOCK) {
    DEBUG("I2C: Initializing...");

    // Set the prescaler to 1
    TWSR &= ~(1 << TWPS0);
    TWSR &= ~(1 << TWPS1);

    // Set the bit rate to 100kHz
    TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;

    // Enable the TWI module
    TWCR |= (1 << TWEN);

    DEBUG("I2C: Initialized!");
}

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

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

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

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

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

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

    DEBUG("Start: 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
    DEBUG("Stop: Sending stop condition");
    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);

    // Wait for the stop condition to be sent
    DEBUG("Stop: Waiting for stop condition to be sent");
    while (TWCR & (1 << TWSTO))
        ;

    DEBUG("Stop: Stop condition sent");
}

uint8_t I2C_write(uint8_t data) {
    // Load the data into the data register
    TWDR = data;

    // Start transmission of data
    DEBUG("Write: Sending data");
    TWCR = (1 << TWINT) | (1 << TWEN);

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

    // Return true if the data was sent
    DEBUG("Write: Data transmission complete");
    return (TWSR & 0xF8) == TW_MT_DATA_ACK;
}

uint8_t I2C_read(uint8_t ack) {
    DEBUG("Read: Waiting for data to be received");
    // 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)
    DEBUG("Read: Waiting for TWI to finish current job");
    while (!(TWCR & (1 << TWINT)))
        ;

    DEBUG("Read: Data received");
    // Return received data
    return TWDR;
}