#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)) && ((TWSR & 0xf8) != (TW_REP_START))) { 
    #ifdef DEBUG_BUILD
      printf("Couldn't set start condition? TWSR: 0x%x\r\n", TWSR & 0xf8);
    #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)) && (TWSR & 0xf8) != (TW_MR_SLA_ACK)) { 
    #ifdef DEBUG_BUILD
      printf("unhandled NACK in address transmission\
          TWCR: 0x%x\
          TWSR: 0x%x\r\n", TWCR, (TWSR & ~(0b11)));
    #endif
    error();
  }

}

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

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\r\n", TWCR, (TWSR & ~(0b11)));
    #endif
    error();
  }
}

uint8_t i2c_recv() {
  uint8_t value;
  TWCR = _BV(TWINT) | _BV(TWEN); 
  loop_until_bit_is_set(TWCR, TWINT);
  value = TWDR;
  //the eeprom supposedly doesn't return an ack here, 
  //as it should (according to its datasheet), but it still works?
  if(((TWSR & 0xf8) != TW_MR_DATA_ACK) && ((TWSR & 0xf8) != TW_MR_DATA_NACK)) { 
#ifdef DEBUG_BUILD
    printf("Error recieving byte from i2c device: 0x%x\r\n", TWSR & 0xf8);
#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();
}

void i2c_write_reg_addr16(uint8_t device_addr, uint16_t device_reg, uint8_t data) {
  i2c_start(device_addr, I2C_WRITE);
  i2c_send((uint8_t)device_reg & 0xff);
  i2c_send((uint8_t)device_reg >> 8);
  i2c_send(data);
  i2c_stop();
}