/*
  si2c.c - Software I2C library for ESP31B

  Copyright (c) 2015 Hristo Gochkov. All rights reserved.
  This file is part of the ESP31B core for Arduino environment.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/
#include <stdint.h>
#include <stdbool.h>
#include "twi.h"
#include "soc/gpio_reg.h"
#include <stdio.h>
#include "esp32-hal-gpio.h"

unsigned char twi_dcount = 18;
static unsigned char twi_sda, twi_scl;

static inline void SDA_LOW()
{
  //Enable SDA (becomes output and since GPO is 0 for the pin,
  // it will pull the line low)
  if (twi_sda < 32)
  {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, BIT(twi_sda));
  }
  else
  {
    REG_WRITE(GPIO_ENABLE1_W1TS_REG, BIT(twi_sda - 32));
  }
}

static inline void SDA_HIGH()
{
  //Disable SDA (becomes input and since it has pullup it will go high)
  if (twi_sda < 32)
  {
    REG_WRITE(GPIO_ENABLE_W1TC_REG, BIT(twi_sda));
  }
  else
  {
    REG_WRITE(GPIO_ENABLE1_W1TC_REG, BIT(twi_sda - 32));
  }
}

static inline uint32_t SDA_READ()
{
  if (twi_sda < 32)
  {
    return (REG_READ(GPIO_IN_REG) & BIT(twi_sda)) != 0;
  }
  else
  {
    return (REG_READ(GPIO_IN1_REG) & BIT(twi_sda - 32)) != 0;
  }
}

static void SCL_LOW()
{
  if (twi_scl < 32)
  {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, BIT(twi_scl));
  }
  else
  {
    REG_WRITE(GPIO_ENABLE1_W1TS_REG, BIT(twi_scl - 32));
  }
}

static void SCL_HIGH()
{
  if (twi_scl < 32)
  {
    REG_WRITE(GPIO_ENABLE_W1TC_REG, BIT(twi_scl));
  }
  else
  {
    REG_WRITE(GPIO_ENABLE1_W1TC_REG, BIT(twi_scl - 32));
  }
}

static uint32_t SCL_READ()
{
  if (twi_scl < 32)
  {
    return (REG_READ(GPIO_IN_REG) & BIT(twi_scl)) != 0;
  }
  else
  {
    return (REG_READ(GPIO_IN1_REG) & BIT(twi_scl - 32)) != 0;
  }
}

#ifndef FCPU80
#define FCPU80 80000000L
#endif

#if F_CPU == FCPU80
#define TWI_CLOCK_STRETCH 800
#else
#define TWI_CLOCK_STRETCH 1600
#endif

void twi_setClock(unsigned int freq)
{
#if F_CPU == FCPU80
  if (freq <= 100000)
    twi_dcount = 19; //about 100KHz
  else if (freq <= 200000)
    twi_dcount = 8; //about 200KHz
  else if (freq <= 300000)
    twi_dcount = 3; //about 300KHz
  else if (freq <= 400000)
    twi_dcount = 1; //about 400KHz
  else
    twi_dcount = 1; //about 400KHz
#else
  if (freq <= 100000)
    twi_dcount = 32; //about 100KHz
  else if (freq <= 200000)
    twi_dcount = 14; //about 200KHz
  else if (freq <= 300000)
    twi_dcount = 8; //about 300KHz
  else if (freq <= 400000)
    twi_dcount = 5; //about 400KHz
  else if (freq <= 500000)
    twi_dcount = 3; //about 500KHz
  else if (freq <= 600000)
    twi_dcount = 2; //about 600KHz
  else
    twi_dcount = 1; //about 700KHz
#endif
}

void i2c_pinMode(int pin, int mode)
{
  gpio_config_t conf = {0};
  conf.pin_bit_mask = 1LL << pin;
  if (mode == OUTPUT)
  {
    conf.mode = GPIO_MODE_OUTPUT;
  }
  if (mode == INPUT || mode == INPUT_PULLUP)
  {
    conf.mode = GPIO_MODE_INPUT;
  }
  if (mode == INPUT)
  {
    conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    conf.pull_up_en = GPIO_PULLUP_DISABLE;
  }
  else if (mode == INPUT_PULLUP)
  {
    conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    conf.pull_up_en = GPIO_PULLUP_ENABLE;
  }
  gpio_config(&conf);
}

void twi_init(unsigned char sda, unsigned char scl)
{
  twi_sda = sda;
  twi_scl = scl;
  i2c_pinMode(twi_sda, OUTPUT);
  i2c_pinMode(twi_scl, OUTPUT);

  gpio_set_level(twi_sda, 0);
  gpio_set_level(twi_scl, 0);

  i2c_pinMode(twi_sda, INPUT_PULLUP);
  i2c_pinMode(twi_scl, INPUT_PULLUP);
  twi_setClock(100000);
}

void twi_stop(void)
{
  i2c_pinMode(twi_sda, INPUT);
  i2c_pinMode(twi_scl, INPUT);
}

static void twi_delay(unsigned char v)
{
  unsigned int i;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  unsigned int reg;
  for (i = 0; i < v; i++)
    reg = REG_READ(GPIO_IN_REG);
#pragma GCC diagnostic pop
}

static bool twi_write_start(void)
{
  SCL_HIGH();
  SDA_HIGH();
  if (SDA_READ() == 0)
    return false;
  twi_delay(twi_dcount);
  SDA_LOW();
  twi_delay(twi_dcount);
  return true;
}

static bool twi_write_stop(void)
{
  unsigned int i = 0;
  SCL_LOW();
  SDA_LOW();
  twi_delay(twi_dcount);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH)
    ; // Clock stretching (up to 100us)
  twi_delay(twi_dcount);
  SDA_HIGH();
  twi_delay(twi_dcount);

  return true;
}

bool do_log = false;
static bool twi_write_bit(bool bit)
{
  unsigned int i = 0;
  SCL_LOW();
  if (bit)
  {
    SDA_HIGH();
    if (do_log)
    {
      twi_delay(twi_dcount + 1);
    }
  }
  else
  {
    SDA_LOW();
    if (do_log)
    {
    }
  }
  twi_delay(twi_dcount + 1);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH)
    ; // Clock stretching (up to 100us)
  twi_delay(twi_dcount);
  return true;
}

static bool twi_read_bit(void)
{
  unsigned int i = 0;
  SCL_LOW();
  SDA_HIGH();
  twi_delay(twi_dcount + 2);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH)
    ; // Clock stretching (up to 100us)
  bool bit = SDA_READ();
  twi_delay(twi_dcount);
  return bit;
}

static bool twi_write_byte(unsigned char byte)
{

  if (byte == 0x43)
  {
    // printf("TWB %02x ", (uint32_t) byte);
    // do_log = true;
  }
  unsigned char bit;
  for (bit = 0; bit < 8; bit++)
  {
    twi_write_bit((byte & 0x80) != 0);
    byte <<= 1;
  }
  if (do_log)
  {
    printf("\n");
    do_log = false;
  }
  return !twi_read_bit(); //NACK/ACK
}

static unsigned char twi_read_byte(bool nack)
{
  unsigned char byte = 0;
  unsigned char bit;
  for (bit = 0; bit < 8; bit++)
    byte = (byte << 1) | twi_read_bit();
  twi_write_bit(nack);
  return byte;
}

unsigned char twi_writeTo(unsigned char address, unsigned char *buf, unsigned int len, unsigned char sendStop)
{
  unsigned int i;
  if (!twi_write_start())
    return 4; //line busy
  if (!twi_write_byte(((address << 1) | 0) & 0xFF))
  {
    if (sendStop)
      twi_write_stop();
    return 2; //received NACK on transmit of address
  }
  for (i = 0; i < len; i++)
  {
    if (!twi_write_byte(buf[i]))
    {
      if (sendStop)
        twi_write_stop();
      return 3; //received NACK on transmit of data
    }
  }
  if (sendStop)
    twi_write_stop();
  i = 0;
  while (SDA_READ() == 0 && (i++) < 10)
  {
    SCL_LOW();
    twi_delay(twi_dcount);
    SCL_HIGH();
    twi_delay(twi_dcount);
  }
  return 0;
}

unsigned char twi_readFrom(unsigned char address, unsigned char *buf, unsigned int len, unsigned char sendStop)
{
  unsigned int i;
  if (!twi_write_start())
    return 4; //line busy
  if (!twi_write_byte(((address << 1) | 1) & 0xFF))
  {
    if (sendStop)
      twi_write_stop();
    return 2; //received NACK on transmit of address
  }
  for (i = 0; i < (len - 1); i++)
    buf[i] = twi_read_byte(false);
  buf[len - 1] = twi_read_byte(true);
  if (sendStop)
    twi_write_stop();
  i = 0;
  while (SDA_READ() == 0 && (i++) < 10)
  {
    SCL_LOW();
    twi_delay(twi_dcount);
    SCL_HIGH();
    twi_delay(twi_dcount);
  }
  return 0;
}
