#include "twi.h"

void twi_init() {
    // bitrate = 1600000MHz / (16 + 2 * TWBR * 4^TWPS)

    // Overall prescaler 40 = 16 + 2 * 3 * 4^1
    TWBR = 3;               // 2 * 3 = 6
    TWSR |= (1 << TWPS0);   // 4

}

uint8_t twi_start(void) {
    // Enable internal pull-ups
    PORTC |= (1 << PC5) | (1 << PC4);

    // Setup for sending start condition, trigger sending, keep TWI enabled
    TWCR |= (1 << TWSTA) | (1 << TWINT) | (1 << TWEN);

    // Poll for TWI unit to finish
    while(!(TWCR & (1 << TWINT)));

    // Check if start was succesfull
    return (TW_STATUS != TW_START);
}

void twi_stop(void) {
    // Setup for sending stop condition, trigger sending, keep TWI enabled
    TWCR = (1 << TWSTO) | (1 << TWINT) | (1 << TWEN);
}


uint8_t twi_write(uint8_t address, uint8_t data[], uint8_t len) {
    // Send start condition
    if(twi_start()) {
        return 1;
    }

    TWDR = (address << 1) | TW_WRITE;       // We want to write to the slave
    TWCR = (1 << TWINT) | (1 << TWEN);  // Trigger transmission, keep TWI enabled

    // Wait for TWI unit to finish
    while(!(TWCR & (1 << TWINT)));

    // Make sure the slave has acked
    if(TW_STATUS != TW_MT_SLA_ACK) {
        return 1;
    }

    for(uint8_t i = 0; i < len; i++) {
        TWDR = data[i];                     // Send actual data
        TWCR = (1 << TWINT ) | (1 << TWEN); // Trigger transmission, keep TWI enabled

        // Wait for TWI unit to finish
        while(!(TWCR & (1 << TWINT)));

        // Make sure the data has been acked
        if(TW_STATUS != TW_MT_DATA_ACK) {
            return 1;
        }
    }

    // Send stop condition
    twi_stop();

    return 0;
}