#include <stdlib.h>
#include <inttypes.h>
#include "Arduino.h"
#include "twi.h"
#include "mik32_hal_irq.h"

#define TIMEOUT_TICKS   1000000
#define TWI_FREQ_DEF    WIRE_FREQ_100K  // default is 100 kHz

// ---------------------------------------------------------------- //

/* 
 * Function twi_init
 * Desc     readys twi pins and sets twi bitrate
 * Input    handler - pointer to wire common handler, 
 *          slaveAddress - address of Arduino if working in slave mode
 * Output   none
 */
uint8_t twi_init(WireHandler_TypeDef* handler, uint8_t slaveAddress)
{
  // Common settings
  uint32_t EPICmask;

  if (handler->i2c_num == 0)
  {
    handler->i2c_param.Instance = I2C_0;
    EPICmask = HAL_EPIC_I2C_0_MASK;
  }
  else if (handler->i2c_num == 1)
  {
    handler->i2c_param.Instance = I2C_1;
    EPICmask = HAL_EPIC_I2C_1_MASK;
  }
  else
    return I2C_ERROR;

  handler->i2c_param.Init.DigitalFilter = I2C_DIGITALFILTER_2CLOCKCYCLES;
  handler->i2c_param.Init.AnalogFilter  = I2C_ANALOGFILTER_DISABLE;
  if (slaveAddress == 0)  // if there is no address - master mode
  {
    handler->i2c_param.Init.Mode    = HAL_I2C_MODE_MASTER;
    handler->i2c_param.Init.AutoEnd = I2C_AUTOEND_ENABLE;
  }
  else
  {
    // 7-bit address without any additional bits
    handler->i2c_param.Init.Mode          = HAL_I2C_MODE_SLAVE;
    handler->i2c_param.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; // stretch is used
    handler->i2c_param.Init.OwnAddress1   = slaveAddress;
  }
  
  // set the frequency
  if (handler->i2c_param.Init.Mode == HAL_I2C_MODE_MASTER)
    twi_setFrequency(handler, TWI_FREQ_DEF, true);
  else
    twi_setFrequency(handler, 0, true);

  if (HAL_I2C_Init(&(handler->i2c_param)) != HAL_OK)
    return I2C_ERROR;

  // enable interrupts for slave mode
  if (handler->i2c_param.Init.Mode == HAL_I2C_MODE_SLAVE)
  {
    // enable interrupts for addressing, byte reception and transfer completion
    HAL_I2C_InterruptDisable(&(handler->i2c_param), I2C_INTMASK);
    HAL_I2C_InterruptEnable(&(handler->i2c_param), I2C_CR1_ADDRIE_M | I2C_CR1_RXIE_M | I2C_CR1_STOPIE_M);        
    // enable interrupt from i2c
    HAL_EPIC_MaskLevelSet(EPICmask);
  }
  handler->isInited = true;
  return I2C_OK;
}

/* 
 * Function twi_deinit
 * Desc     disables twi pins
 * Input    handler - pointer to wire common handler
 * Output   none
 */
void twi_deinit(WireHandler_TypeDef* handler)
{
  HAL_I2C_Deinit(&(handler->i2c_param));

  // for slave mode disable interrupts from i2c
  if (handler->i2c_param.Init.Mode == HAL_I2C_MODE_SLAVE) 
  {
    if (handler->i2c_num == 0)
      HAL_EPIC_MaskLevelClear(HAL_EPIC_I2C_0_MASK);
    else
      HAL_EPIC_MaskLevelClear(HAL_EPIC_I2C_1_MASK);
  }

  handler->isInited = false;
}

/* 
 * Function twi_setClock
 * Desc     sets twi bit rate
 * Input    handler - pointer to wire common handler
 *          Clock Frequency - 100000 / 400000 / 1000000, 
 *          onInit = true only if function is called from twi_init()
 * Output   I2C_OK - frequency changed, I2C_ERROR - frequency didn't change
 */
uint8_t twi_setFrequency(WireHandler_TypeDef* handler, uint32_t frequency, bool onInit)
{
  uint8_t ret = I2C_OK;
  
  // change the frequency only during or after bus initialization
  if (handler->isInited || onInit)
  {
    // You can change the frequency only when the interface is turned off
    HAL_I2C_Disable(&(handler->i2c_param));
  
    if (frequency == WIRE_FREQ_100K)      // 100 kHz
    {
      handler->i2c_param.Clock.PRESC  = 2;
      handler->i2c_param.Clock.SCLDEL = 8;
      handler->i2c_param.Clock.SDADEL = 2;
      handler->i2c_param.Clock.SCLH   = 49;
      handler->i2c_param.Clock.SCLL   = 49;
      // CurrentFrequency = WIRE_FREQ_100K;
    }
    else if (frequency == WIRE_FREQ_400K) // 400 kHz
    {
      handler->i2c_param.Clock.PRESC  = 0;
      handler->i2c_param.Clock.SCLDEL = 3;
      handler->i2c_param.Clock.SDADEL = 2;
      handler->i2c_param.Clock.SCLH   = 30;
      handler->i2c_param.Clock.SCLL   = 30;
      // CurrentFrequency = WIRE_FREQ_400K;
    }
    else if (frequency == WIRE_FREQ_1000K)// 1000 kHz
    {
      handler->i2c_param.Clock.PRESC  = 0;
      handler->i2c_param.Clock.SCLDEL = 1;
      handler->i2c_param.Clock.SDADEL = 2;
      handler->i2c_param.Clock.SCLH   = 6;
      handler->i2c_param.Clock.SCLL   = 6;
      // CurrentFrequency = WIRE_FREQ_1000K;
    }
    else if (frequency == 0)              // slave mode
    {
      handler->i2c_param.Clock.PRESC  = 0; 
      handler->i2c_param.Clock.SCLDEL = 0;
      handler->i2c_param.Clock.SDADEL = 2;
      handler->i2c_param.Clock.SCLH   = 0;
      handler->i2c_param.Clock.SCLL   = 0;
    }
    else
      // frequency does not change
      ret = I2C_ERROR;
    
    // write the timings to the register if everything is OK
    if (ret == I2C_OK) 
      HAL_I2C_SetClockSpeed(&(handler->i2c_param));

    //turn the interface back only if initialization has already passed
    if (handler->isInited)
      HAL_I2C_Enable(&(handler->i2c_param));
  }
  else
    ret = I2C_ERROR;
  
  return ret;
}

/* 
 * Function twi_masterReadFrom
 * Desc     attempts to become twi bus master and read a
 *          series of bytes from a device on the bus
 * Input    hi2c: pointer to hardware i2c handler
 *          address: 7bit i2c device address
 *          data: pointer to byte array
 *          length: number of bytes to read into array
 *          sendStop: Boolean indicating whether to send a stop at the end
 * Output   number of bytes read
 */
uint8_t twi_masterReadFrom(I2C_HandleTypeDef* hi2c, uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
{
  uint8_t ret = 0;

  // if there are errors left from previous transactions, you need to restart 
  // the interface for correct operation
  if (hi2c->ErrorCode != I2C_ERROR_NONE)
    HAL_I2C_Reset(hi2c);

  if (sendStop)   hi2c->Init.AutoEnd = 1;
  else            hi2c->Init.AutoEnd = 0;
  
  // put the data directly into the external buffer
  if (HAL_I2C_Master_Receive(hi2c, address, data, length, TIMEOUT_TICKS) == HAL_OK)
    ret = length;

  return ret; 
}

/* 
 * Function twi_masterWriteTo
 * Desc     attempts to become twi bus master and write a
 *          series of bytes to a device on the bus
 * Input    hi2c: pointer to hardware i2c handler
 *          address: 7bit i2c device address
 *          data: pointer to byte array
 *          length: number of bytes in array
 *          sendStop: boolean indicating whether or not to send a stop at the end
 * Output   0 .. success
 *          1 .. length to long for buffer
 *          2 .. address send, NACK received
 *          3 .. data send, NACK received
 *          4 .. other twi error (lost bus arbitration, bus error, ..)
 *          5 .. timeout
 */
uint8_t twi_masterWriteTo(I2C_HandleTypeDef* hi2c, uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
{
  uint8_t ret = I2C_OK;

  // if there are errors left from previous transactions, you need to restart 
  // the interface for correct operation
  if (hi2c->ErrorCode != I2C_ERROR_NONE)
    HAL_I2C_Reset(hi2c);
    
  if (sendStop)   hi2c->Init.AutoEnd = 1;
  else            hi2c->Init.AutoEnd = 0;
  
  // take data from an external buffer
  HAL_I2C_Master_Transmit(hi2c, address, data, length, TIMEOUT_TICKS);

  // parse errors
  // check separately, because in hal libraries not all functions look at this
  if (HAL_I2C_Get_Interrupts_Status(hi2c) & I2C_ISR_NACKF_M) 
    hi2c->ErrorCode = I2C_ERROR_NACK;
  if      (hi2c->ErrorCode == (HAL_I2C_ErrorTypeDef)I2C_ERROR_TIMEOUT) ret = I2C_TIMEOUT;    // timeout
  else if (hi2c->ErrorCode == (HAL_I2C_ErrorTypeDef)I2C_ERROR_NACK)    ret = I2C_NACK_DATA;  // didn't receive ACK
  else if (hi2c->ErrorCode != (HAL_I2C_ErrorTypeDef)I2C_OK)            ret = I2C_ERROR;      // any other error

  return ret;
}

/* 
 * Function twi_slaveWrite
 * Desc     attempts to become twi bus slave and write a
 *          series of bytes to a master after it asks
 * Input    hi2c: pointer to hardware i2c handler
 *          txData: pointer to byte array
 *          bytesNum: number of bytes in array
 * Output   0 .. success
 *          5 .. timeout
 */
i2c_status_e twi_slaveWrite(I2C_HandleTypeDef* hi2c, uint8_t *txData, uint8_t bytesNum)
{
  if ((hi2c->ErrorCode != I2C_ERROR_NONE))
    HAL_I2C_Reset(hi2c);

  // send data
  HAL_StatusTypeDef error_code = HAL_OK;
  HAL_I2C_Clear_Reload(hi2c);
  if (!(HAL_I2C_Get_CR1_Content(hi2c) & I2C_CR1_NOSTRETCH_M))  // NOSTRETCH = 0 
    HAL_I2C_Reset_TXDR_Content(hi2c);
  HAL_I2C_Write_TXDR(hi2c, txData[0]); // first recording is made in advance
  
  // write byte 
  for (uint32_t tx_count = 1; tx_count < bytesNum; tx_count++)
  {
      if ((error_code = HAL_I2C_Slave_WaitTXIS(hi2c, TIMEOUT_TICKS)) != HAL_OK)
      {
          // failed to write
          HAL_I2C_Reset_TXDR_Content(hi2c);
          HAL_I2C_Reset_Interrupt_Flag(hi2c, I2C_ICR_STOPCF_M); // Clear the STOP detection flag on the bus 
          HAL_I2C_Reset(hi2c);
          return I2C_TIMEOUT;
      }
      HAL_I2C_Write_TXDR(hi2c, txData[tx_count]);
  }

  if ((error_code = HAL_I2C_WaitBusy(hi2c, TIMEOUT_TICKS)) != HAL_OK)
  {
      // failed to complete transaction
      HAL_I2C_Reset(hi2c);
      return I2C_TIMEOUT;
  }
  HAL_I2C_Reset_TXDR_Content(hi2c);    
  HAL_I2C_Reset_Interrupt_Flag(hi2c, I2C_ICR_STOPCF_M); // Clear the STOP detection flag on the bus 
  
  return I2C_OK;
}

/* 
 * Function twi_interruptHandler
 * Desc     handles an interrupts from twi
 * Input    handler - pointer to wire common handler
 * Output   none
 */
void __attribute__((optimize("O3"))) twi_interruptHandler(WireHandler_TypeDef* handler)
{
  uint32_t int_mask = HAL_I2C_Get_CR1_Content(&(handler->i2c_param)) & I2C_INTMASK; // interrupts allowed
  uint32_t interrupt_status = HAL_I2C_Get_Interrupts_Status(&(handler->i2c_param)); // current flags

  // master calls by address, device in slave mode
  if ((interrupt_status & I2C_ISR_ADDR_M) && (int_mask & I2C_CR1_ADDRIE_M))
  {
      // reset ADDR flag
      HAL_I2C_Reset_Interrupt_Flag(&(handler->i2c_param), I2C_ICR_ADDRCF_M);

      // look at the transmission direction and respond to the request
      if (interrupt_status & I2C_ISR_DIR_M) // master reads, slave sends
      {
        // twi_onSlaveTransmit(); // slave send data
        if (handler->onSlaveTransmit)
          handler->onSlaveTransmit(handler->instance);
      }
      else                                  // master writes, slave reads
      {
        handler->rxBufferIndex = 0; // write from the beginning of the buffer
        handler->i2c_param.State = HAL_I2C_STATE_BUSY;
        HAL_I2C_Clear_Reload(&(handler->i2c_param));
        // wait for interrupts by receiving a byte or a stop condition
      }
  }

  // new byte of data received
  if ((interrupt_status & I2C_ISR_RXNE_M) && (int_mask & I2C_CR1_RXIE_M))
  {
    // put new byte into buffer
    handler->rxBuffer[handler->rxBufferIndex++] = HAL_I2C_Get_RXDR(&(handler->i2c_param));
  }
  
  // master sent a STOP to the bus
  if ((interrupt_status & I2C_ISR_STOPF_M) && (int_mask & I2C_CR1_STOPIE_M))
  {
    handler->i2c_param.State = HAL_I2C_STATE_END;
    HAL_I2C_Reset_TXDR_Content(&(handler->i2c_param));
    HAL_I2C_Reset_Interrupt_Flag(&(handler->i2c_param), I2C_ICR_STOPCF_M); // Clear the STOP detection flag on the bus 
    // pass the received data to callback function
    handler->onSlaveReceive(handler->instance, handler->rxBuffer, handler->rxBufferIndex);
  }
}