#include "i2c.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) {
  TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTA);
  
  while(!(TWCR & (1<<TWINT)));
      
  if (TW_STATUS != TW_START) return false;
  
  TWDR = (addr&0xFE);
  TWCR = (1<<TWEN)|(1<<TWINT);
  
  while(!(TWCR & (1<<TWINT)));
  
  if(TW_STATUS == TW_MT_SLA_ACK)
    return true;
  
  I2CStop();
  return false;
}

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

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) {
      I2CStop();
      return false;
    }
  }
  
  I2CStop();
  return true;
}

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