#include "i2c.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

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

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) {
    i2c_recv[i2c_recv_len] = data;
    i2c_recv_len++;
}

void i2c_read_handler() {
    printf("Master wanted to read.\n");
}

void i2c_stop_handler() {
    // Reset counter and clear receive buffer
    i2c_recv[i2c_recv_len] = '\0';
    printf("%s\n", i2c_recv);
    i2c_reset_recv();
}

// Address received
ISR(TWI0_TWIS_vect) {
    // 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;
        }
        
        // 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;
        }
    }
}