#include "i2c.h"

I2C::I2C() 
{
  TWBR = 64;			// I2C bitrate (must be 10 or higher)
  TWCR = 4+64;			// TWI enable bit + ackn  
  TWSR = 0;				// prescaler

  // I2C pull-ups are set in the board file //
  
  err=0;
}

uint8_t I2C::error()
{
  return err;
}


void I2C::setSlaveAdress(uint8_t address)
{
  TWAR = address<<1;   // slave address (shift by one since bit 0 has different meaning)
}


bool I2C::isAction()
{
  if (TWCR&(1<<TWINT)) return true;
  else return false;
}


inline
bool I2C::isActionGet()
{
  return ((TWSR&248)==TW_ST_SLA_ACK);    // own I2C address received, read mode, ACK sent
}


inline
bool I2C::isActionSend()
{
  return ((TWSR&248)==TW_SR_SLA_ACK);    // own I2C address received, write mode, ACK sent
}


void I2C::sendStartSLA_W(uint8_t address)
{
  bool OK = false;
  int counter = 0;
  
  do {
 
	TWCR |= BV(TWINT)|BV(TWSTA);				// send start condition
    while(!(TWCR&BV(TWINT)));					// wait for OK
	
    if ((TWSR&248)!=TW_START) {				// start condition was sent  -> OK
	  err=ERROR_NO_START;
	  return;
	}

    TWDR=(address<<1);							// slave address + write mode
    TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA);	// generate command

    while(!(TWCR&128)) {};					// wait for OK
    if ((TWSR&248)==TW_MT_SLA_ACK) OK=true;   // SLA+W was sent, ACK received  -> OK
    if ((TWSR&248)==TW_MT_SLA_NACK) {			// SLA+W was sent, ACK not received -> ERROR
      counter++;
	  
	  if (counter == 10) {						// After 10 tries...
        err=ERROR_NO_ACK;						// ... return with ERROR_NO_ACK
	    return;
      }
	}

  } while(!OK);
  
  err=0;								// OK
}


void I2C::sendStartSLA_R(uint8_t address)
{
  bool OK=false;
  while (!OK) {
	TWCR |= BV(TWINT)|BV(TWSTA);				// send start condition
//    sbi(TWCR, TWSTA);               // generate start condition
    while(!(TWCR&128));             // wait for OK
    if ((TWSR&248)==8);             // start condition was sent  -> OK

    TWDR=(address<<1) | 1;          // slave address + read mode
    TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA);           // generate command

    while(!(TWCR&128));             // wait for send OK + ACK/NACK from slave
    if ((TWSR&248)==0x38);          // Arbitration lost or NOT ACK -> ERROR
    if ((TWSR&248)==0x40) OK=true;  // SLA+R was sent, ACK received  -> OK
    if ((TWSR&248)==0x48);          // SLA+W was sent, ACK not received -> ERROR
    sbi(TWCR, TWINT);               // OK
  }
}

// This is used by send() called by the master //
void I2C::sendByte(uint8_t data)
{
  TWDR = data;                    			// send one data byte
  TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);  // start transmission

  while(!(TWCR&(1<<TWINT)));				// wait for TWCR to become 0
  if ((TWSR&248)==TW_MT_DATA_ACK);        // data sent, ACK received -> OK
  if ((TWSR&248)==TW_MT_DATA_NACK);       // data sent, ACK not received -> ERROR
}


void I2C::sendStop()
{
  TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA) | BV(TWSTO);       // generate stop command
}

// Note: in qfix, the I2C address is the board identifier 
// and the first data byte ist the logical ID
inline
void I2C::send(uint8_t address, uint8_t data)
{
  send(address, &data, 1);
}


// Note: in qfix, the I2C address is the board identifier 
// and the first data byte ist the logical ID
void I2C::send(uint8_t address, uint8_t* data, int length)
{
  sendStartSLA_W(address);
  if (err!=0) 
  {
	sendStop();
	//err = ERROR_NOT_SENT;	
	return;
  }

  for (int i=0; i<length; i++) {
    sendByte(data[i]);
  }
  sendStop();
  err = 0;					// OK
}



int I2C::receive(uint8_t* data)
{
  TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);

  while(!(TWCR&(1<<TWINT)));				// wait for something

  int len = 0;
  while ((TWSR&248)==TW_SR_DATA_ACK) {		// data received (in TWDR), ACK sent 
    data[len] = TWDR;						// read received byte
    len++;
    TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
    while(!(TWCR&(1<<TWINT))) { }			// wait for OK
  }

  if ((TWSR&248)==TW_SR_STOP) { }			// STOP (or new START) received -> OK
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);           // generate command

  return len;
}


void I2C::readByte(uint8_t& data)
{
  while(!isAction());

  if ((TWSR&248)==0x50);      // data received: NACK 
  if ((TWSR&248)==0x58);      // data received: ACK
  data = TWDR;
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);           // generate command
}


void I2C::get(uint8_t address, uint8_t* data, int length)
{
  sendStartSLA_R(address);
  if (err!=0) 
  {
	sendStop();
	err = ERROR_NOT_SENT;	
	return;
  }
  
  for (int i=0; i<length; i++) {
    readByte(data[i]);
  }
  sendStop();
}




void I2C::returnBytes(uint8_t* data, int len, bool lastOne)
{
  for (int i=0; i<len; i++) {
    TWDR=data[i];                  // byte to send

    if (i==len-1) TWCR = (1<<TWINT) | (1<<TWEN);     // command for last byte (no TWEA is correct!)
    else TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);   // command for all others

    while(!(TWCR&(1<<TWINT)));         // wait for OK

    if ((TWSR&248)==0xB8);      // data sent, ACK ACK received -> OK
    if ((TWSR&248)==0xC0);      // data sent, ACK not received -> ERROR
  }
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);           // generate command
}