#include "TWI.h"

namespace TWI {
    BusState startMessage(Address addr) {
        TWCR = (1 << TWSTA) | (1 << TWEN) | (1 << TWINT);
        wait();
        uint8_t status = sendByte(addr);

        if (status == 0x18 || status == 0x40) return TWI_OK;
        else return status;
    }

    void close() {
        TWCR = (1 << TWSTO) | (1 << TWEN) | (1 << TWINT);
    }


    BusState sendByte(uint8_t byte) {
        TWDR = byte;
        TWCR = (1 << TWEN) | (1 << TWINT);
        wait();
        uint8_t status = (TWSR & 0xF8);
        if (status != 0x28 && status != 0x30) return status;
        else return TWI_OK;
    }

    void wait() {
        while (!(TWCR & (1 << TWINT)));
    }

    BusState nack() {
        TWCR = (1 << TWEN) | (1 << TWINT);
        wait();
        uint8_t status = (TWSR & 0xF8);
        if (status != 0x50 && status != 0x58) return status;
        else return TWI_OK;
    }

    BusState ack() {
        TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWEA);
        wait();
        uint8_t status = (TWSR & 0xF8);
        if (status != 0x50 && status != 0x58) return status;
        else return TWI_OK;
    }

    uint8_t readByte() {
        return TWDR;
    }

    void init(uint32_t busSpeed) {

        // TWSR = ~(1 << TWPS0);
        // TWSR = ~(1 << TWPS1);
        TWBR = ((F_CPU / busSpeed - 16) / (2 * 4));


/*
    // initialize twi prescaler and bit rate
    cbi(TWSR, TWPS0);
    cbi(TWSR, TWPS1);
    TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;*/

        /* twi bit rate formula from atmega128 manual pg 204
        SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
        note: TWBR should be 10 or higher for master mode
        It is 72 for a 16mhz Wiring board with 100kHz TWI */

        // enable twi module, acks, and twi interrupt
        TWCR = _BV(TWEN) | _BV(TWEA); //| _BV(TWIE);

        //


    }

}