#include <stdio.h>
#include <avr/io.h>
#include <util/twi.h>
#include <util/delay.h>
#include <stdbool.h>
#include <string.h>
#ifdef DEBUG_BUILD
#include "debug.h"
#endif

#include "i2c.h"


/** I2C notes
 * TWDR: data address shift register
 *  contians address or data bytes to be transmitted/recieved
 *  ACK bit not accesible
 * TWAR: matches recieved addresses
 * TWCR: control register, sets settings
  *  TWINT: tw interrupt flag 
  *  also is dependent on GIE in SREG
  * TWSR: TWI status register (only matters if TWI interurupt flag asserted)
  
 * Steps simplified:
 * SEND START
  * Send START with TWCR
  * Set TWINT in TWCR to 1; as this clears the flag
  * TWSR will then say start has begun
 * Load SLA+W into TWDR (what is this? TODO)
 * Write something to TWCR to initiate send? (make sure twint is set after)
 * poll TWSR interrupt bit to see if we recieve a message (or figure out interrupts)
 * 
 * Note: You may be wondering why I'm not handling i2c errors.
 * I have limited time, and I don't think it's nessesary unless I start running into them.
 *
 * Some I may work on handling later.
 * 
  */

void error() {}

void i2c_init() {
  TWBR = 12;
  TWSR &= 0b11111100;
}


void i2c_start(uint8_t addr, bool rw) {
  TWCR = (_BV(TWSTA) | _BV(TWEN) | _BV(TWINT)); //send start signal
  loop_until_bit_is_set(TWCR, TWINT);
  if((TWSR & 0xf8) != TW_START) { 
    #ifdef DEBUG_BUILD
      printf("Couldn't set start condition?\n");
    #endif
    error();
  }

  TWDR = (addr << 1) | rw;
  TWCR = _BV(TWINT) | _BV(TWEN); 
  loop_until_bit_is_set(TWCR, TWINT);

  if((TWSR & 0xf8) != TW_MT_SLA_ACK) { 
    #ifdef DEBUG_BUILD
      printf("unhandled NACK in address transmission\
          TWCR: 0x%x\
          TWSR: 0x%x\n", TWCR, (TWSR & ~(0b11)));
    #endif
    error();
  }

}

void i2c_stop() { TWCR = (_BV(TWSTO) | _BV(TWEN) | _BV(TWINT)); }

void i2c_send(uint8_t byte) {
  TWDR = byte; //fist packet is address 
  TWCR = _BV(TWINT) | _BV(TWEN); //send over the byte
  loop_until_bit_is_set(TWCR, TWINT);
 
  if((TWSR & 0xf8) != TW_MT_DATA_ACK) { 
    #ifdef DEBUG_BUILD
      printf("unhandled NACK in data transmission\
          TWCR: 0x%x\
          TWSR: 0x%x\n", TWCR, (TWSR & ~(0b11)));
    #endif
    error();
  }
}

uint8_t i2c_recv() {
  uint8_t value;
  loop_until_bit_is_set(TWCR, TWINT);
  value = TWDR;
  if((TWSR & 0xf8) != TW_MR_SLA_ACK) {
#ifdef DEBUG_BUILD
    printf("Error recieving byte from i2c device\n");
#endif
  error();
  }
  return value;
}

//does NOT control start/stop, simply reads TWDR

uint8_t i2c_read_reg_addr16(uint8_t device, uint16_t addr) {
  uint8_t value;

  i2c_start(device, I2C_WRITE); 
  i2c_send((uint8_t)addr & 0xff);
  i2c_send((uint8_t)addr >> 8);
  i2c_start(device, I2C_READ);
  value = i2c_recv();
  i2c_stop();

  return(value);
}

void i2c_write_reg(uint8_t device_addr, uint8_t device_reg, uint8_t value) {
  i2c_start(device_addr, I2C_WRITE);
  i2c_send(device_reg);
  i2c_send(value);
  i2c_stop();
}

void i2c_write_reg_multi(uint8_t device_addr, uint8_t device_reg, size_t len, uint8_t *values) {
  i2c_start(device_addr, I2C_WRITE);
  i2c_send(device_reg);
  for(size_t on_byte = 0; on_byte < len; on_byte++) i2c_send(values[on_byte]);
  i2c_stop();
}