#include "i2c.h"
#include "avr.h"

#include <util/twi.h>


static bool I2CStartSend(uint8_t addr);
static bool I2CStartRecv(uint8_t addr);
static void I2CStop();


static bool I2CStartSend(uint8_t addr) {
  while(true) {
    TWCR |= (1<<TWINT)|(1<<TWSTA);
    
    while(!(TWCR & (1<<TWINT)));
    
    if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
    
    TWDR = (addr&0xFE);
    TWCR = (1<<TWEN)|(1<<TWINT);
    
    while(!(TWCR & (1<<TWINT)));
    
    if(TW_STATUS == TW_MT_SLA_ACK)
      return true;
    
    I2CStop();
  }
}

static bool I2CStartRecv(uint8_t addr) {
  while(true) {
    TWCR |= (1<<TWINT)|(1<<TWSTA);
    
    while(!(TWCR & (1<<TWINT)));
        
    if(TW_STATUS != TW_START && TW_STATUS != TW_REP_START) continue;
    
    TWDR = (addr|1);
    TWCR |= (1<<TWINT);
    
    while(!(TWCR & (1<<TWINT)));
    
    if(TW_STATUS == TW_MR_SLA_ACK)
      return true;
    
    I2CStop();
  }
}

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


void initI2C() {
  TWBR = 10;
  TWCR = (1<<TWEN);
  TWSR = 0;
}

bool I2CSendByte(uint8_t addr, uint8_t data) {
  if(!I2CStartSend(addr)) return false;
  
  TWDR=data;
  
  TWCR = (1<<TWEN)|(1<<TWINT);
  while(!(TWCR & (1<<TWINT)));
  
  if(TW_STATUS != TW_MT_DATA_ACK) {
    I2CStop();
    return false;
  }
  
  I2CStop();
  return true;
}

bool I2CSend(uint8_t addr, uint8_t *data, int length) {
  if(!I2CStartSend(addr)) return false;
  
  for(int i = 0; i < length; i++) {
    TWDR=data[i];
  
    TWCR = (1<<TWEN)|(1<<TWINT);
    while(!(TWCR & (1<<TWINT)));
    
    if(TW_STATUS != TW_MT_DATA_ACK && i < length-1) {
      I2CStop();
      return false;
    }
  }
  
  I2CStop();
  return true;
}

bool I2CRecvByte(uint8_t addr, uint8_t *data) {
  if(!I2CStartRecv(addr)) return false;
  
  TWCR = (1<<TWEN)|(1<<TWINT);
  
  while(!(TWCR & (1<<TWINT)));
  
  if(TW_STATUS != TW_MR_DATA_ACK && TW_STATUS != TW_MR_DATA_NACK) {
    I2CStop();
    return false;
  }
  
  *data = TWDR;
  TWCR = (1<<TWEN)|(1<<TWINT);
  
  I2CStop();
  
  return true;
}

int I2CRecv(uint8_t addr, uint8_t *data, int length) {
  if(!I2CStartRecv(addr)) return -1;
  
  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
  
  for(int i = 0; i < length-1; i++) {
    while(!(TWCR & (1<<TWINT)));
    
    switch(TW_STATUS) {
      case TW_MR_DATA_ACK:
        data[i] = TWDR;
        break;
      case TW_MR_DATA_NACK:
        data[i] = TWDR;
        I2CStop();
        return i+1;
      default:
        I2CStop();
        return -1;
    }
  }
  
  TWCR = (1<<TWEN)|(1<<TWINT);
  while(!(TWCR & (1<<TWINT)));
  
  switch(TW_STATUS) {
    case TW_MR_DATA_ACK:
    case TW_MR_DATA_NACK:
      data[length-1] = TWDR;
      I2CStop();
      return length;
  }
  
  I2CStop();
  return -1;
}