#include "i2c.h"
#include "command-handler.h"

// Basic I2C handling structure is heavily inspired by:
// https://github.com/microchip-pic-avr-examples/avr128db48-bare-metal-twi-mplab/blob/master/twi-client.X/peripherals/TWI/TWI_client.c

// We need to keep track of if the stop confition is in regards to a write or
// read request
volatile bool last_action_write = false;

volatile uint8_t i2c_recv[I2C_RECV_BUF_SIZE] = {
    0}; // Arbitrary length array to hold the received
        // data, longer than max expected command
volatile uint8_t i2c_recv_len = 0;

void init_i2c(void) {
  // Pin setup
  PORTA.DIRSET = PIN2_bm | PIN3_bm;
  PORTA.PINCTRLUPD = PIN2_bm | PIN3_bm;

  // Enable operating in debug
  TWI0.DBGCTRL = TWI_DBGRUN_bm;

  // Initialize the control A register
  TWI0.CTRLA = TWI_INPUTLVL_I2C_gc    // I2C voltage transition level
               | TWI_SDASETUP_4CYC_gc // Four clock cycles setup time
               | TWI_SDAHOLD_50NS_gc  // 50ns SDA hold time
               | TWI_FMPEN_OFF_gc     // Standard SPI timing
      ;

  // The device's slave address
  TWI0.SADDR = 0x42;

  // Enable acting as a slave
  TWI0.SCTRLA = TWI_DIEN_bm     // Enable data interrupt
                | TWI_APIEN_bm  // Enable more interrupts
                | TWI_PIEN_bm   // Enable stop flag interrupt
                | PIN2_bm       // Respond to all TWI addresses
                | TWI_ENABLE_bm // Enable acting as a slave
      ;

  // Reset the received stuff
  i2c_reset_recv();
}

// Reset received counter and clear receive buffer
void i2c_reset_recv() {
  i2c_recv_len = 0;
  for (int i = 0; i < I2C_RECV_BUF_SIZE; i++) {
    i2c_recv[i] = 0;
  }
}

void i2c_write_handler(uint8_t data) {
  last_action_write = true;
  i2c_recv[i2c_recv_len] = data;
  i2c_recv_len++;
}

void i2c_read_handler() {
  last_action_write = false;
  TWI0.SDATA = route_command(i2c_recv_len);
  i2c_recv_len++; // Increment the counter of the current amount of requests
}

void i2c_stop_handler() {
  if (last_action_write) {
    // Parse the received command data
    parse_command(i2c_recv, i2c_recv_len);
    
    if (i2c_recv[0] == 0x22) {
        route_command(0);
    }
  }
  
  /* Write only commands need to be routed now
  switch (i2c_recv[0]) {
      WRITE_CONFIG:
      CLEAR_BULK_FAN_SPEED:
        route_command(0);
        break;
      default:
          break;
  }
  */
  
  // Reset the buffer for future transmissions
  i2c_reset_recv();
}

// Address received
ISR(TWI0_TWIS_vect) {
  // Disable interrupts while handling I2C
  cli();

  // Check for the data interrupt flag
  if (TWI0.SSTATUS & TWI_DIF_bm) {
    uint8_t data = 0;

    if (((TWI0.SSTATUS & TWI_DIR_bm) >> TWI_DIR_bp) == 0) {
      // Data write Master -> Slave
      data = TWI0.SDATA;

      // Send the data to the write handler
      i2c_write_handler(data);
    } else {
      // Data read Master <- Slave
      i2c_read_handler();
      // data = TWI0.SDATA;
      // TWI0.SDATA = 1;
    }

    // Acknowledge having received
    TWI0.SCTRLB = TWI_ACKACT_ACK_gc | TWI_SCMD_RESPONSE_gc;
  }

  // Check for address match or STOP
  if (TWI0.SSTATUS & TWI_APIF_bm) {

    if (TWI0.SSTATUS & TWI_AP_ADR_gc) {
      // Address match, just send ack
      TWI0.SCTRLB = TWI_ACKACT_ACK_gc | TWI_SCMD_RESPONSE_gc;
    } else {
      // STOP condition received
      i2c_stop_handler();
      // Send ACK
      TWI0.SCTRLB = TWI_ACKACT_ACK_gc | TWI_SCMD_RESPONSE_gc;
    }
  }

  // Re-enable interrupts
  sei();
}