#include <oulib.h>
#include <i2c_test.h>

/////////////////////////////////////////////////////////////////////////////
/*
  See theory of operation documentation in i2c_master_test.c

  The interrupt routine will only transmit the most recent object that is
  placed within the buffer (an object is composed of some number of
  bytes).  Once the object has been transmitted (and no others have
  been inserted), then the interrupt routine will respond by
  indicating that there exists no object (zero bytes to transmit).

  We assume that the during the time that an object is being
  transmitted, that the client code does not insert more than
  NBUFFERS-1 objects.

*/
/////////////////////////////////////////////////////////////////////////////

#define NBUFFERS 2


#define I2C_SLAVE_STATE_IDLE 0
#define I2C_SLAVE_STATE_BYTES 1
#define I2C_SLAVE_STATE_WLAST 2

volatile int8_t slave_current_buffer;
int8_t slave_current_buffer_internal;

volatile uint8_t slave_buffer[NBUFFERS][BUFFER_SIZE];
volatile uint8_t slave_buffer_size[NBUFFERS];
volatile uint8_t slave_state;
uint8_t slave_counter;


ISR(TWI_vect) {
  uint8_t status;
  switch(slave_state)
    {
    case I2C_SLAVE_STATE_IDLE:
      // We have been addressed 
      status = twi_get_status();
      if(status == TW_ST_SLA_ACK) {
	// Store the current buffer reference to a local copy
	slave_current_buffer_internal = slave_current_buffer;

	// Now prepare to transmit the number of bytes
	slave_state = I2C_SLAVE_STATE_BYTES;
	twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
	if(slave_current_buffer_internal == -1){
	  // No bytes to send: send dummy byte
	  twi_send_byte(0x0);
	}else{
	  // Send the number of bytes in the buffer
	  twi_send_byte(slave_buffer_size[slave_current_buffer_internal]);
	};
	slave_counter = 0;
      }else{
	// An error has occurred: reset
	slave_state = I2C_SLAVE_STATE_IDLE;
	twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
	twi_set_twint();
      };
      break;
    case I2C_SLAVE_STATE_BYTES:
      // Transmitting bytes
      status = twi_get_status();
      if(status == TW_ST_DATA_ACK) {
	// Last byte was acknowledged
	if(slave_current_buffer_internal == -1 ||
	   slave_buffer_size[slave_current_buffer_internal] == 0) {
	  // Send a dummy byte
	  twi_set_ack(TWI_ACKNOWLEDGE_DISABLE);  // Disable indicates last byte
	  twi_send_byte(0x0);
	  slave_state = I2C_SLAVE_STATE_WLAST;
	}else{
	  // Send a byte
	  if(slave_counter + 1 ==
	     slave_buffer_size[slave_current_buffer_internal]) {
	    // This is the last byte
	    twi_set_ack(TWI_ACKNOWLEDGE_DISABLE);
	    slave_state = I2C_SLAVE_STATE_WLAST;
	  }else{
	    // This is not the last byte
	    twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
	  };
	  // Queue up that next byte
	  twi_send_byte(slave_buffer[slave_current_buffer_internal][slave_counter]);
	  ++slave_counter;
	};
      }else{
	// An error
	slave_state = I2C_SLAVE_STATE_IDLE;
	twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
	twi_set_twint();
      };
      break;
    case I2C_SLAVE_STATE_WLAST:
      // We are waiting for the NACK for the last byte
      status = twi_get_status();
      /*
      if(status == TW_ST_DATA_NACK) {
	//DEBUG_PORT = DEBUG_LED2;
      }else{
	// An error has occurred: reset
	//DEBUG_PORT = DEBUG_LED2|DEBUG_LED1;
      };
      */

      // Reset the FSM
      slave_state = I2C_SLAVE_STATE_IDLE;
      twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
      twi_set_twint();

      // If the next message has not been added to the buffer, then
      //  we don't have any more to send
      if(slave_current_buffer == slave_current_buffer_internal) {
	slave_current_buffer = -1;
      };
      break;
    };
};


void i2c_slave_init(uint8_t address)
{
  // Turn on TWI
  twi_set_state(_BV(TWEN));

  twi_set_address(address);
  //slave_state = I2C_MASTER_STATE_DONE;
  twi_set_rate(0x80);
  twi_set_prescalar(TWI_PRESCALAR_64);
  twi_interrupt_config(TWI_INTERRUPT_ENABLE);

  // Nothing to send
  slave_current_buffer = slave_current_buffer_internal = -1;  
  slave_state = I2C_SLAVE_STATE_IDLE;


  // Ready to ack immediately
  twi_set_ack(TWI_ACKNOWLEDGE_ENABLE);
  sei();
};


uint8_t i2c_add_buffer(uint8_t size, uint8_t *buf)
{
  uint8_t i;

  if(size > BUFFER_SIZE) return(0);  // Error condition
  
  // Which buffer to put the data in?
  // Must deal with the race condition (slave_current_buffer is
  //   altered by the isr)
  int8_t buffer = slave_current_buffer;
  if(buffer == -1) buffer = 0;          // First one
  else buffer = (buffer + 1)%NBUFFERS;  // Next one

  // Copy buffer contents
  for(i=0; i < size; ++i) {
    slave_buffer[buffer][i] = buf[i];
  };
  slave_buffer_size[buffer] = size;
  
  // Critical section: set new buffer
  cli();
  slave_current_buffer = buffer;
  sei();
  return(1);
};


/////////////////////////////////////////////////////////////////////////////

void send_test(void)
{
  //uint8_t status;
  uint8_t buf[BUFFER_SIZE];

  buf[0] = 0x5a;
  buf[1] = 0xf0;
  buf[2] = 0x0f;
  buf[3] = 0xa5;

  while(1) {
    // Indicate that we are just about to send the objecdt
    DEBUG_PORT = 0xf;
    delay_ms(100);
    DEBUG_PORT = PINC & 0x3;
    // Place new item in buffer.  The length of the item is
    //  determined by the state of C1 and C0
    i2c_add_buffer(PINC & 0x3, buf);
    delay_ms(2000);

  };
};


void address_test(void)
{
  while(1) {
    /*
    if(twi_get_state() & _BV(TWINT)) {
      DEBUG_PORT ^= DEBUG_LED0;
    }else{
      DEBUG_PORT ^= DEBUG_LED1;
    };
    */
    delay_ms(200);
  };
};


int main(void)

{
  PORTB = 0;
  DEBUG_PORT_DDR = DEBUG_LED0 | DEBUG_LED1 | DEBUG_LED2;

  i2c_slave_init(0x10);
  send_test();
  //address_test();
}