#include "project.h"
#include <stdio.h>
#include <string.h>

#define SDA_BANK GPIOB
#define SDA_GPIO GPIO7

#define SCL_BANK GPIOB
#define SCL_GPIO GPIO6


#define I2C_READ    1
#define I2C_WRITE   0

#define OUR_ADDRESS 0x90


typedef enum {
  ST_LOST = 0,
  ST_ADDRESS_7,
  ST_ADDRESS_6,
  ST_ADDRESS_5,
  ST_ADDRESS_4,
  ST_ADDRESS_3,
  ST_ADDRESS_2,
  ST_ADDRESS_1,
  ST_ADDRESS_0,
  ST_ADDRESS_ACK,
  ST_OUT_7,
  ST_OUT_6,
  ST_OUT_5,
  ST_OUT_4,
  ST_OUT_3,
  ST_OUT_2,
  ST_OUT_1,
  ST_OUT_0,
  ST_PREP_OUT_ACK,
  ST_OUT_ACK,
  /*Doesn't fall through*/
  ST_PREP_REG_7,
  ST_REG_7,
  ST_REG_6,
  ST_REG_5,
  ST_REG_4,
  ST_REG_3,
  ST_REG_2,
  ST_REG_1,
  ST_REG_0,
  ST_REG_ACK,
  ST_PREP_IN_7,
  ST_IN_7,
  ST_IN_6,
  ST_IN_5,
  ST_IN_4,
  ST_IN_3,
  ST_IN_2,
  ST_IN_1,
  ST_IN_0,
  ST_IN_ACK,
  ST_REG_WRITE,
  /*Doesn't fall through*/


} i2c_state_t;


static int dsda;

static void i2c_bb_sda (int sda)
{
  dsda = sda;

  if (sda) {
    gpio_set_mode (SDA_BANK, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, SDA_GPIO);
    gpio_set (SDA_BANK, SDA_GPIO);
  } else {
    gpio_set_mode (SDA_BANK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_OPENDRAIN, SDA_GPIO);
    gpio_clear (SDA_BANK, SDA_GPIO);
  }
}


static void i2c_bb_scl (int scl)
{
  if (scl) {
    gpio_set_mode (SCL_BANK, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, SCL_GPIO);
    gpio_set (SCL_BANK, SCL_GPIO);
  } else {
    gpio_set_mode (SCL_BANK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_OPENDRAIN, SCL_GPIO);
    gpio_clear (SCL_BANK, SCL_GPIO);
  }
}

static i2c_state_t state = ST_LOST;

static void i2c_bb_sm (int od, int d, int oc, int c)
{
  static int rnw ;
  static uint8_t sr;
  static uint8_t reg;

  //Check for START
  if (oc && c && od && !d) {
    state = ST_ADDRESS_7;
    sr = 0;
  }

  // Check for STOP
  if (oc && c && !od && d) {
    state = ST_LOST;
  }

  switch (state) {
  case ST_LOST:
    i2c_bb_sda (1);
    i2c_bb_scl (1);
    break;
  case ST_ADDRESS_7...ST_ADDRESS_0:

    if (! (!oc && c)) {
      break;
    }

    //rising scl of a7...a0
    sr = (sr << 1) | !!d;

    if (state == ST_ADDRESS_0) {
      if ( (sr & ~I2C_READ) == OUR_ADDRESS) {
        state++;
        rnw = d;
      } else {
        state = ST_LOST;
      }
    } else {
      state++;
    }

    break;
  case ST_ADDRESS_ACK:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of a0
    i2c_bb_sda (0);

    if (rnw) {
      state++;
    } else {
      state = ST_PREP_REG_7;
    }

    break;
  case ST_OUT_7:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of ack
    i2c_bb_sda (1);
    //stretch SCL
    i2c_bb_scl (0);
    sr = vuart_read (reg);
    i2c_bb_sda (sr & 0x80);
    sr <<= 1;
    i2c_bb_scl (1);
    state++;
    break;
  case ST_OUT_6...ST_OUT_0:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of d6...d0
    i2c_bb_sda (sr & 0x80);
    sr <<= 1;
    state++;
    break;
  case ST_PREP_OUT_ACK:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of d0
    i2c_bb_sda (1);
    state++;
    break;
  case ST_OUT_ACK:

    if (! (!oc && c)) {
      break;
    }

    //rising scl of ack

    if (d) {
      /*They're done with us */
      state = ST_LOST;
    } else {
      /*They want more */
      state = ST_OUT_7;
      reg++;
    }

    break;
  case ST_PREP_REG_7:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of ack
    i2c_bb_sda (1);
    sr = 0;
    state++;
    break;
  case ST_REG_7...ST_REG_0:

    if (! (!oc && c)) {
      break;
    }

    //rising scl of r7..r0
    sr = (sr << 1) | !!d;
    state++;
    break;
  case ST_REG_ACK:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of r0
    reg = sr;
    i2c_bb_sda (0);
    state = ST_PREP_IN_7;
    break;
  case ST_PREP_IN_7:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of ack
    i2c_bb_sda (1);
    sr = 0;
    state++;
    break;
  case ST_IN_7...ST_IN_0:

    if (! (!oc && c)) {
      break;
    }

    //rising scl of i7..i0
    sr = (sr << 1) | !!d;
    state++;
    break;
  case ST_IN_ACK:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of i0
    i2c_bb_sda (0);
    state++;
    break;
  case ST_REG_WRITE:

    if (! (oc && !c)) {
      break;
    }

    //falling scl of ack
    i2c_bb_sda (1);
    //stretch SCL
    i2c_bb_scl (0);
    vuart_write (reg, sr);
    i2c_bb_scl (1);
    reg++;
    sr = 0;
    state = ST_IN_7;
    break;
  default:
    state = ST_LOST;
  }
}



void i2c_bb_init (void)
{
  i2c_bb_scl (1);
  i2c_bb_sda (1);
}


void i2c_bb_poll (void)
{
  int sda = gpio_get (SDA_BANK, SDA_GPIO);
  int scl = gpio_get (SCL_BANK, SCL_GPIO);
  static int old_sda = 1, old_scl = 1;
  i2c_bb_sm (old_sda, sda, old_scl, scl);
  old_scl = scl;
  old_sda = sda;
}