/*
 * Copyright (C) 2020-2021 Aloysius Indrayanto
 *
 * This file is part of the eCxx library, see LICENSE file for the license details.
 */

/*
 * ----------------------------------------------------------------------------------
 * The code in this file is copied (with a few modifications) from:
 *     core_esp8266_si2c.cpp - software I2C library for esp8266
 * ----------------------------------------------------------------------------------
 * Copyright (C) 2015 Hristo Gochkov. All rights reserved.
 *
 * This file is part of the esp8266 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 <user_interface.h>

#include "CoreUtil.h"
#include "PolledTimeout.h"
#include "SoftTWIDriver.h"
#include "TWIDefs.h"


//
// TWI status code
//
#define TWI_OK                      0

#define TWI_SCL_HELD_LOW            1
#define TWI_SCL_HELD_LOW_AFTER_READ 2

#define TWI_SDA_HELD_LOW            3
#define TWI_SDA_HELD_LOW_AFTER_INIT 4


namespace eCxx {

namespace NodeMCU {


//
// Software TWI driver
//
         uint32_t                       SoftTWIDriver::preferredTWIClock            = TWIMaster::DefaultTWIFrequency;
         uint8_t                        SoftTWIDriver::sdaPin                       = 0;
         uint8_t                        SoftTWIDriver::sclPin                       = 0;
         uint8_t                        SoftTWIDriver::twiAddr                      = 0;
         uint32_t                       SoftTWIDriver::twiClockStretchLimit         = 0;
         uint32_t                       SoftTWIDriver::twiDCount                    = 18;

volatile SoftTWIDriver::TWIPMode        SoftTWIDriver::twipMode                     = SoftTWIDriver::TWIPM_IDLE;
volatile SoftTWIDriver::TWIPState       SoftTWIDriver::twipState                    = SoftTWIDriver::TWIP_IDLE;
volatile int32_t                        SoftTWIDriver::twipStatus                   = TW_NO_INFO;
volatile int32_t                        SoftTWIDriver::bitCount                     = 0;

volatile uint8_t                        SoftTWIDriver::twiData                      = 0;
volatile bool                           SoftTWIDriver::twiAck                       = false;
volatile bool                           SoftTWIDriver::twiAckRec                    = false;
volatile int32_t                        SoftTWIDriver::twiTimeoutMS                 = 10;

volatile SoftTWIDriver::TWIState        SoftTWIDriver::twistate                     = SoftTWIDriver::TWI_READY;
volatile uint8_t                        SoftTWIDriver::twiError                     = 0xFF;

         uint8_t                        SoftTWIDriver::txBuffer[SoftTWIDriverBufferSize];
volatile int32_t                        SoftTWIDriver::txBufferIndex                = 0;
volatile int32_t                        SoftTWIDriver::txBufferLength               = 0;

         uint8_t                        SoftTWIDriver::rxBuffer[SoftTWIDriverBufferSize];
volatile int32_t                        SoftTWIDriver::rxBufferIndex                = 0;

         ETSEvent                       SoftTWIDriver::eventTaskQueue[SoftTWIQueueSize];
         ETSTimer                       SoftTWIDriver::timer;

         SoftTWIDriver::OnSlaveTransmit SoftTWIDriver::onSlaveTransmit              = nullptr;
         SoftTWIDriver::OnSlaveReceive  SoftTWIDriver::onSlaveReceive               = nullptr;

         bool                           SoftTWIDriver::slaveEnabled                 = false;


void SoftTWIDriver::setClock(uint32_t freq)
{
    // Minimum freq 1000 Hz to minimize slave timeouts and WDT resets
    if(freq < 1000) freq = 1000;

    preferredTWIClock = freq;

    if( system_get_cpu_freq() == SYS_CPU_80MHZ ) {
        if(freq > 400000UL) freq = 400000UL;
        twiDCount = 500000000UL / freq;                          // half-cycle period in nS
        twiDCount = ( 1000UL * (twiDCount - 1120UL) ) / 62500UL; // ( (half-cycle - overhead) / busyWait ) loop time
    }

    else {
        if(freq > 800000UL) freq = 800000UL;
        twiDCount = 500000000UL / freq;                          // half-cycle period in nS
        twiDCount = ( 1000UL * (twiDCount -  560UL) ) / 31250UL; // ( (half-cycle - overhead) / busyWait ) loop time
    }
}


void SoftTWIDriver::setClockStretchLimit(uint32_t limit)
{ twiClockStretchLimit = limit; }


void SoftTWIDriver::init(uint8_t sda, uint8_t scl)
{
    // Set timer function
    ets_timer_setfn(&timer, onTimer, NULL);

    // Create event task
    ets_task(eventTask, SoftTWITaskPriority, eventTaskQueue, SoftTWIQueueSize);

    // Configure pins
    sdaPin = sda;
    sclPin = scl;

    NodeMCU::pinMode(sdaPin, INPUT_PULLUP);
    NodeMCU::pinMode(sclPin, INPUT_PULLUP);

    // Set clock parameters
    setClock(preferredTWIClock);
    setClockStretchLimit(150000UL); // Default value is 150 mS
}


void SoftTWIDriver::setAddress(uint8_t address)
{ twiAddr = address << 1; } // Set TWI slave address (skip over the R/W bit)


bool SoftTWIDriver::enableSlave()
{
    // Nothing more to do if slave mode is already enabled
    if(slaveEnabled) return true;

    // Error if the pin(s) are not yet set
    if(!sdaPin || !sclPin) return false;

    // Attach interrupt
    attachInterrupt(sdaPin, &SoftTWIDriver::onSdaChange, InterruptMode::Change);
    attachInterrupt(sclPin, &SoftTWIDriver::onSclChange, InterruptMode::Change);

    // Set flag
    slaveEnabled = true;

    return true;
}


bool SoftTWIDriver::writeStart()
{
    SCL_HIGH(sclPin);
    SDA_HIGH(sdaPin);

    if( !SDA_READ(sdaPin) ) return false;

    busyWait(twiDCount);

    SDA_LOW(sdaPin);

    busyWait(twiDCount);

    return true;
}


bool SoftTWIDriver::writeStop()
{
    SCL_LOW(sclPin);
    SDA_LOW(sdaPin);

    busyWait(twiDCount);

    SCL_HIGH(sclPin);
    WAIT_CLOCK_STRETCH();

    busyWait(twiDCount);

    SDA_HIGH(sdaPin);

    busyWait(twiDCount);

    return true;
}


bool SoftTWIDriver::writeBit(bool bit)
{
    SCL_LOW(sclPin);

    if(bit) SDA_HIGH(sdaPin);
    else    SDA_LOW(sdaPin);

    busyWait(twiDCount + 1);

    SCL_HIGH(sclPin);
    WAIT_CLOCK_STRETCH();

    busyWait(twiDCount);

    return true;
}


bool SoftTWIDriver::readBit()
{
    SCL_LOW(sclPin);
    SDA_HIGH(sdaPin);

    busyWait(twiDCount + 2);

    SCL_HIGH(sclPin);
    WAIT_CLOCK_STRETCH();

    const bool bit = SDA_READ(sdaPin);

    busyWait(twiDCount);

    return bit;
}


bool SoftTWIDriver::writeByte(uint8_t byte)
{
    for(uint32_t bit = 0; bit < 8; ++bit) {
        writeBit(byte & 0x80);
        byte <<= 1;
    }

    return !readBit(); // Return ACK or NACK
}


uint8_t SoftTWIDriver::readByte(bool nack)
{
    uint8_t byte = 0;

    for(uint32_t bit = 0; bit < 8; ++bit) {
        byte = (byte << 1) | readBit();
    }

    writeBit(nack);

    return byte;
}


void SoftTWIDriver::sclPinGenValley()
{
    SCL_LOW(sclPin);

    busyWait(twiDCount);

    SCL_HIGH(sclPin);
    WAIT_CLOCK_STRETCH();
}


TWIResultCode SoftTWIDriver::writeTo(uint8_t address, uint8_t* buf, uint32_t len, uint8_t sendStop)
{
    if( !writeStart() ) return TWIResultCode::OtherError; // Line busy

    if( !writeByte( ( (address << 1) | 0x00 ) & 0xFF ) ) {
        if(sendStop) writeStop();
        return TWIResultCode::AddressNACK; // Received NACK on transmit of address
    }

    for(uint32_t i = 0; i < len; ++i) {
        if( !writeByte(buf[i]) ) {
            if(sendStop) writeStop();
            return TWIResultCode::DataNACK; // Received NACK on transmit of data
        }
    }

    if(sendStop) {
        writeStop();
    }
    else {
        sclPinGenValley();
        //busyWait(twiDCount);
    }

    for(uint32_t i = 0; i < 10; ++i) {
        if( SDA_READ(sdaPin) ) break;
        sclPinGenValley();
        busyWait(twiDCount);
    }

    return TWIResultCode::NoError;
}


TWIResultCode SoftTWIDriver::readFrom(uint8_t address, uint8_t* buf, uint32_t len, uint8_t sendStop)
{
    if( !writeStart() ) return TWIResultCode::OtherError; // Line busy

    if( !writeByte( ( (address << 1) | 0x01 ) & 0xFF ) ) {
        if(sendStop) writeStop();
        return TWIResultCode::AddressNACK; // Received NACK on transmit of address
    }

    for(uint32_t i = 0; i < (len - 1); ++i) {
        buf[i] = readByte(false);
    }
    buf[len - 1] = readByte(true);

    if(sendStop) {
        writeStop();
    }
    else {
        sclPinGenValley();
        //busyWait(twiDCount);
    }

    for(uint32_t i = 0; i < 10; ++i) {
        if( SDA_READ(sdaPin) ) break;
        sclPinGenValley();
        busyWait(twiDCount);
    }

    return TWIResultCode::NoError;
}


uint8_t SoftTWIDriver::status()
{
    // Wait for a slow slave to finish
    WAIT_CLOCK_STRETCH();

    // SCL held low by another device, no procedure available to recover
    if( !SCL_READ(sclPin) ) return TWI_SCL_HELD_LOW;

    // If SDA low, read the bits slaves have to sent to a max
    uint32_t clockCount = 20;
    while( !SDA_READ(sdaPin) && clockCount-- > 0 ) {
        readBit();
        if( !SCL_READ(sclPin) ) {
            // I2C bus error; SCL held low beyond slave clock stretch time
            return TWI_SCL_HELD_LOW_AFTER_READ;
        }
    }

    if( !SDA_READ(sdaPin) ) {
        // I2C bus error; SDA line held low by slave/another_master after n bits.
        return TWI_SDA_HELD_LOW;
    }

    return TWI_OK;
}


TWIResultCode SoftTWIDriver::transmit(const uint8_t* data, uint8_t length)
{
    // Check if there is enough space in the buffer
    if(SoftTWIDriverBufferSize < txBufferLength + length) return TWIResultCode::DataTooLong;

    // Ensure we are currently a slave transmitter
    if(twistate != TWI_STX) return TWIResultCode::NotSlaveTx;

    // Copy data into Tx buffer
    for(uint32_t i = 0; i < length; ++i) txBuffer[txBufferLength++] = data[i];

    return TWIResultCode::NoError;
}


void SoftTWIDriver::attachSlaveRxEvent(OnSlaveReceive func)
{ onSlaveReceive = func; }


void SoftTWIDriver::attachSlaveTxEvent(OnSlaveTransmit func)
{ onSlaveTransmit = func; }


void ICACHE_RAM_ATTR SoftTWIDriver::reply(bool ack)
{
    // Transmit master read ready signal, with or without ack
    if(ack) {
        SCL_HIGH(sclPin);
        twiAck = true;
    }
    else {
        SCL_HIGH(sclPin);
        twiAck = false;
    }
}


void ICACHE_RAM_ATTR SoftTWIDriver::releaseBus()
{
    // Release bus
    SCL_HIGH(sclPin);
    twiAck = true;
    SDA_HIGH(sdaPin);

    // Update twi state
    twistate = TWI_READY;
}


void ICACHE_RAM_ATTR SoftTWIDriver::busyWait(uint32_t v)
{
    // Loop time is 5 machine cycles: 31.25ns @ 160 MHz ; 62.5ns @ 80 MHz
    for(uint32_t i = 0; i < v; ++i) {
        __asm__ __volatile__ ( "nop \n\t" );
    }
}


void ICACHE_RAM_ATTR SoftTWIDriver::onTwipEvent(uint8_t status)
{
    twipStatus = status;

    switch(status) {

        // ### ### ### All ### ### ###

        // No state information available
        case TW_NO_INFO:
            break;

        // Illegal start or stop condition
        case TW_BUS_ERROR:
            twiError = TW_BUS_ERROR;
            break;

        // ### ### ### Slave transmitter ### ### ###

        // SLA+R received, ACK returned
        case TW_ST_SLA_ACK:
            /* FALLTHROUGH */

        // Arbitration lost in SLA+RW, SLA+R received, ACK returned
        case TW_ST_ARB_LOST_SLA_ACK:
            // Enter slave transmitter mode
            twistate = TWI_STX;
            // Ready the Tx buffer index for iteration
            txBufferIndex = 0;
            // Set Tx buffer length to be zero, to verify if user changes it
            txBufferLength = 0;
            // Callback to user-defined callback over event task to allow for non-RAM-residing code;
            // the user is expected to call transmit() to do this
            ets_post(SoftTWITaskPriority, TWI_SIG_TX, 0);
            break;

        // Data transmitted, ACK received
        case TW_ST_DATA_ACK:
            // Copy data to output register
            twiData  = txBuffer[txBufferIndex++];
            bitCount = 8;
            --bitCount;
            if(twiData & 0x80) SDA_HIGH(sdaPin);
            else               SDA_LOW (sdaPin);
            twiData <<= 1;
            // ACK if there is more to send, NACK otherwise
            if(txBufferIndex < txBufferLength) reply(true );
            else                               reply(false);
            break;

        // Data transmitted, NACK received
        case TW_ST_DATA_NACK:
            /* FALLTHROUGH */

        // Last data byte transmitted, ACK received
        case TW_ST_LAST_DATA:
            // Leave slave receiver state
            releaseBus();
            break;

        // ### ### ### Slave receiver ### ### ###

        // SLA+W received, ACK returned
        case TW_SR_SLA_ACK:
            /* FALLTHROUGH */

        // General call received, ACK returned
        case TW_SR_GCALL_ACK:
            /* FALLTHROUGH */

        // Arbitration lost in SLA+RW, SLA+W received, ACK returned
        case TW_SR_ARB_LOST_SLA_ACK:
            /* FALLTHROUGH */

        // Arbitration lost in SLA+RW, general call received, ACK returned
        case TW_SR_ARB_LOST_GCALL_ACK:
            // Enter slave receiver mode
            twistate = TWI_SRX;
            // Indicate that Rx buffer can be overwritten and ACK
            rxBufferIndex = 0;
            reply(true);
            break;

        // Data received, ACK returned
        case TW_SR_DATA_ACK:
            /* FALLTHROUGH */

        // General call data received, ACK returned
        case TW_SR_GCALL_DATA_ACK:
            // ACK if there is still available space, ...
            if(rxBufferIndex < SoftTWIDriverBufferSize) {
                // Put byte in buffer and ACK
                rxBuffer[rxBufferIndex++] = twiData;
                reply(true);
            }
            else {
                // ... NACK otherwise
                reply(false);
            }
            break;

        // Stop or repeated start condition received while selected
        case TW_SR_STOP:
            // Put a null character after data if there's room
            if(rxBufferIndex < SoftTWIDriverBufferSize) rxBuffer[rxBufferIndex] = '\0';
            // Callback to user-defined callback over event task to allow for non-RAM-residing code
            ets_post(SoftTWITaskPriority, TWI_SIG_RX, rxBufferIndex);
            // Reset the Rx buffer index
            rxBufferIndex = 0;
            break;

        // Data received, NACK returned
        case TW_SR_DATA_NACK:
            /* FALLTHROUGH */

        // General call data received, NACK returned
        case TW_SR_GCALL_DATA_NACK:
            // NACK back at master
            reply(false);
            break;

    } // switch status
}


void ICACHE_RAM_ATTR SoftTWIDriver::onTimer(void*)
{
    releaseBus();

    onTwipEvent(TW_BUS_ERROR);
    twipMode  = TWIPM_WAIT;
    twipState = TWIP_BUS_ERR;
}


// Convert the state machine to 1-hot encoded state machine
#define S2M(X)     _BV(X)
#define IFSTATE(X) if( twipStateMask & (X) )


void ICACHE_RAM_ATTR SoftTWIDriver::onSdaChange()
{
    // Read pins
    const bool sda = SDA_READ(sdaPin);
    const bool scl = SCL_READ(sclPin);

    const uint32_t twipStateMask = S2M(twipState);

    if(scl) {
        IFSTATE( S2M(TWIP_IDLE) ) {
            if(sda) {
                // Ignore
            }
            else {
                // Start
                bitCount  = 8;
                twipState = TWIP_START;
                ets_timer_arm_new(&timer, twiTimeoutMS, false, true); // Once
            }
        }
        else IFSTATE( S2M(TWIP_START) | S2M(TWIP_REP_START) | S2M(TWIP_SEND_ACK) | S2M(TWIP_WAIT_ACK) | S2M(TWIP_SLA_R) | S2M(TWIP_REC_ACK) | S2M(TWIP_READ_ACK) | S2M(TWIP_RWAIT_ACK) | S2M(TWIP_WRITE) ) {
            // Start or stop
            SDA_HIGH(sdaPin); // Should not be necessary
            onTwipEvent(TW_BUS_ERROR);
            twipMode  = TWIPM_WAIT;
            twipState = TWIP_BUS_ERR;
        }
        else IFSTATE( S2M(TWIP_WAIT_STOP) | S2M(TWIP_BUS_ERR) ) {
            if(sda) {
                // Stop
                SCL_LOW(sclPin);  // Generates a low SCL pulse after stop
                ets_timer_disarm(&timer);
                twipState = TWIP_IDLE;
                twipMode  = TWIPM_IDLE;
                SCL_HIGH(sclPin);
            }
            else {
                // Start
                if( twipState == TWIP_BUS_ERR ) {
                    // Ignore
                }
                else {
                    bitCount  = 8;
                    twipState = TWIP_REP_START;
                    ets_timer_arm_new(&timer, twiTimeoutMS, false, true); // Once
                }
            }
        }
        else IFSTATE( S2M(TWIP_SLA_W) | S2M(TWIP_READ) ) {
            // Start or stop
            if(bitCount != 7) {
                // Inside byte transfer - error
                onTwipEvent(TW_BUS_ERROR);
                twipMode  = TWIPM_WAIT;
                twipState = TWIP_BUS_ERR;
            }
            else {
                // During first bit in byte transfer - OK
                SCL_LOW(sclPin); // Clock stretching
                onTwipEvent(TW_SR_STOP);
                if(sda) {
                    // Stop
                    ets_timer_disarm(&timer);
                    twipState = TWIP_IDLE;
                    twipMode = TWIPM_IDLE;
                }
                else
                {
                    // Start
                    bitCount  = 8;
                    ets_timer_arm_new(&timer, twiTimeoutMS, false, true); // Once
                    twipState = TWIP_REP_START;
                    twipMode  = TWIPM_IDLE;
                }
            }
        }
    } // if scl
}


void ICACHE_RAM_ATTR SoftTWIDriver::onSclChange()
{
    // Read pins
    const bool sda = SDA_READ(sdaPin);
    const bool scl = SCL_READ(sclPin);

    // Reset TWI status
    twipStatus = TW_NO_INFO;

    const uint32_t twipStateMask = S2M(twipState);

    IFSTATE( S2M(TWIP_START) | S2M(TWIP_REP_START) | S2M(TWIP_SLA_W) | S2M(TWIP_READ) ) {
        if(!scl) {
            // Ignore
        }
        else {
            twiData <<= 1;
            twiData |= sda;

            --bitCount;
            if(bitCount) {
                // Continue
            }
            else {
                twipState = TWIP_SEND_ACK;
            }
        }
    }
    else IFSTATE( S2M(TWIP_SEND_ACK) ) {
        if(scl) {
            // Ignore
        }
        else {
            if( twipMode == TWIPM_IDLE ) {
                if( (twiData & 0xFE) != twiAddr ) {
                    // Ignore
                }
                else {
                    SDA_LOW(sdaPin);
                }
            }
            else {
                if(!twiAck) {
                    // Ignore
                }
                else {
                    SDA_LOW(sdaPin);
                }
            }
            twipState = TWIP_WAIT_ACK;
        }
    }
    else IFSTATE( S2M(TWIP_WAIT_ACK) ) {
        if(scl) {
            // Ignore
        }
        else {
            if( twipMode == TWIPM_IDLE )
            {
                if( (twiData & 0xFE) != twiAddr ) {
                    SDA_HIGH(sdaPin);
                    twipState = TWIP_WAIT_STOP;
                }
                else {
                    SCL_LOW(sclPin); // Clock stretching
                    SDA_HIGH(sdaPin);
                    twipMode = TWIPM_ADDRESSED;
                    if( !(twiData & 0x01) ) {
                        onTwipEvent(TW_SR_SLA_ACK);
                        bitCount  = 8;
                        twipState = TWIP_SLA_W;
                    }
                    else {
                        onTwipEvent(TW_ST_SLA_ACK);
                        twipState = TWIP_SLA_R;
                    }
                }
            }
            else {
                SCL_LOW(sclPin); // Clock stretching
                SDA_HIGH(sdaPin);
                if( !twiAck ) {
                    onTwipEvent(TW_SR_DATA_NACK);
                    twipMode  = TWIPM_WAIT;
                    twipState = TWIP_WAIT_STOP;
                }
                else {
                    onTwipEvent(TW_SR_DATA_ACK);
                    bitCount  = 8;
                    twipState = TWIP_READ;
                }
            }
        }
    }
    else IFSTATE( S2M(TWIP_SLA_R) | S2M(TWIP_WRITE) ) {
        if(scl) {
            // Ignore
        }
        else
        {
            if(twiData & 0x80) SDA_HIGH(sdaPin);
            else               SDA_LOW(sdaPin);
            twiData <<= 1;

            --bitCount;
            if(bitCount) {
                // Continue
            }
            else {
                twipState = TWIP_REC_ACK;
            }
        }
    }
    else IFSTATE( S2M(TWIP_REC_ACK) ) {
        if(scl) {
            // Ignore
        }
        else {
            SDA_HIGH(sdaPin);
            twipState = TWIP_READ_ACK;
        }
    }
    else IFSTATE( S2M(TWIP_READ_ACK) ) {
        if(!scl) {
            // Ignore
        }
        else {
            twiAckRec = !sda;
            twipState = TWIP_RWAIT_ACK;
        }
    }
    else IFSTATE( S2M(TWIP_RWAIT_ACK) ) {
        if(scl) {
            // Ignore
        }
        else {
            SCL_LOW(sclPin); // Clock stretching
            if( twiAck && twiAckRec ) {
                onTwipEvent(TW_ST_DATA_ACK);
                twipState = TWIP_WRITE;
            }
            else {
                // No more data to send and/or the master does not want anymore
                onTwipEvent(twiAckRec ? TW_ST_LAST_DATA : TW_ST_DATA_NACK);
                twipMode  = TWIPM_WAIT;
                twipState = TWIP_WAIT_STOP;
            }
        }
    }
}


void SoftTWIDriver::eventTask(ETSEvent* e)
{
    if(e == nullptr) return;

    switch(e->sig) {

        case TWI_SIG_TX:
            // Call user callback
            onSlaveTransmit();
            // If the user did not change buffer & length, initialize it
            if(!txBufferLength) {
                txBufferLength = 1;
                txBuffer[0]    = 0x00;
            }
            // Initiate transmission
            onTwipEvent(TW_ST_DATA_ACK);
            break;

        case TWI_SIG_RX:
            // ACK future responses and leave slave receiver state
            releaseBus();
            onSlaveReceive(rxBuffer, e->par);
            break;

    } // switch e->sig
}


} // namespace NodeMCU

} // namespace eCxx
