/*
 * 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_waveform_pwm.cpp - general purpose waveform generation and
 *                                     control; supporting outputs on all pins in
 *                                     parallel
 * ----------------------------------------------------------------------------------
 * Copyright (C) 2018 Earle F. Philhower, III. All rights reserved.
 *
 * 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 "CoreLib.h"
#include "CoreSys.h"


namespace eCxx {

namespace NodeMCU {


//
// Ensure everything is read/written to RAM
//
#define MEMORY_BARRIER() do { __asm__ __volatile__ ( "" ::: "memory" ); } while(0)


//
// Waveform struct
//
struct Waveform {
    uint32_t nextServiceCycle;  // Next cycle timer when a transition required
    uint32_t expiryCycle;       // The cycle when this waveform must stop (for time-limited waveform only)
    uint32_t timeHighCycles;    // Actual running waveform period; adjusted using the desired(High|Low)Cycles variables below
    uint32_t timeLowCycles;     // ---
    uint32_t desiredHighCycles; // Ideal waveform period to drive the error signal
    uint32_t desiredLowCycles;  // ---
    uint32_t lastEdge;          // The cycle when this generator last changed
};


//
// Waveform generator state
//
struct WVFState {
    Waveform waveform[17];          // State of all possible pins

    uint32_t waveformState     = 0; // Inidicates the pin high or low       (updated in NMI so no access outside the NMI code)
    uint32_t waveformEnabled   = 0; // Inidicates if it is actively running (updated in NMI so no access outside the NMI code)

    // Enable lock-free operations by only allowing updates to waveformState and waveformEnabled from IRQ service routine
    uint32_t waveformToEnable  = 0; // Message to the NMI handler to start a waveform on an inactive pin
    uint32_t waveformToDisable = 0; // Message to the NMI handler to disable a pin from waveform generation

    uint32_t waveformToChange  = 0; // Mask of pin to change (one bit set in main application; cleared when effected in the NMI)
    uint32_t waveformNewHigh   = 0;
    uint32_t waveformNewLow    = 0;

    uint32_t (*timer1CB)()     = nullptr;

    // Optimize the NMI inner loop by keeping track of the minimum and maximum GPIO that the system are generating;
    // in the common case (only one PWM), these may be the same pin and the system can avoid looking at the other pins
    uint16_t startPin          = 0;
    uint16_t endPin            = 0;
};

static WVFState wvfState;


//
// PWM state machine
//
constexpr int32_t MAX_PWM = 8;

struct PWMState {
    uint32_t  mask;               // Bitmask of active pins
    uint32_t  cnt;                // How many entries are used
    uint32_t  idx;                // Where the state machine is currently along the list
    uint8_t   pin  [MAX_PWM + 1]; // The list
    uint32_t  delta[MAX_PWM + 1]; // ---
    uint32_t  nextServiceCycle;   // Clock cycle for the next step
    PWMState* pwmUpdate;          // Set by main code, cleared by ISR
};

static PWMState pwmState;

static uint32_t pwmFrequency = 2000;
static uint32_t pwmPeriod    = NodeMCU::microsecondsToClockCycles(1000000UL) / pwmFrequency;


//
// Timer1 interrupt vector
//
static constexpr uint32_t MaximumDelayBetweenIRQs = 10000; // in uS


static inline uint32_t ICACHE_RAM_ATTR earliest(uint32_t a, uint32_t b) __ECXX_ATTR_OPTIMIZE_2;
static inline uint32_t ICACHE_RAM_ATTR earliest(uint32_t a, uint32_t b)
{
    const uint32_t now = NodeMCU::getCycleCountIRQ();
    const int32_t  da  = a - now;
    const int32_t  db  = b - now;

    return (da < db) ? a : b;
}


static void ICACHE_RAM_ATTR timer1Interrupt() __ECXX_ATTR_OPTIMIZE_2;
static void ICACHE_RAM_ATTR timer1Interrupt()
{
    // The SDK and hardware take some time to actually get to the NMI code, so decrement the next IRQ's timer
    // value by a bit so the system can actually catch the real CPU cycle counter needed for the waveforms
    #if F_CPU == 80000000
        #define DELTAIRQ  ( NodeMCU::microsecondsToClockCycles(9) / 4 )
        #define ADJUST(X) ( (X) << (turbo ? 1 : 0) )
    #else
        #define DELTAIRQ  ( NodeMCU::microsecondsToClockCycles(9) / 8 )
        #define ADJUST(X) ( (X) >> 0 )
    #endif

    // When the time to the next edge is greater than this, RTI and set another IRQ to minimize CPU usage
    #define MINIRQTIME NodeMCU::microsecondsToClockCycles(4)

    // Flag if the core is at 160 MHz, for use by ADJUST()
    const bool turbo = ( *reinterpret_cast<uint32_t*>(0x3FF00014) & 0x01 ) ? true : false;

    uint32_t nextEventCycle = NodeMCU::getCycleCountIRQ() + NodeMCU::microsecondsToClockCycles(MaximumDelayBetweenIRQs);
    uint32_t timeoutCycle   = NodeMCU::getCycleCountIRQ() + NodeMCU::microsecondsToClockCycles(14);

    if(wvfState.waveformToEnable || wvfState.waveformToDisable) {
        // Handle enable/disable requests from main application
        wvfState.waveformEnabled    = (wvfState.waveformEnabled & ~wvfState.waveformToDisable) | wvfState.waveformToEnable; // Set the requested waveforms on/off
        wvfState.waveformState     &= ~wvfState.waveformToEnable;                                                           // Clear the state of any just started
        wvfState.waveformToEnable   = 0;
        wvfState.waveformToDisable  = 0;
        // No memory barrier; globals must be written to RAM on ISR exit
        // Find the first GPIO being generated by using GCC's find-first-set
        wvfState.startPin = __builtin_ffs(wvfState.waveformEnabled) - 1;
        // Find the last bit by subtracting off GCC's count-leading-zeros
        wvfState.endPin = 32 - __builtin_clz(wvfState.waveformEnabled);
    }
    else if(!pwmState.cnt && pwmState.pwmUpdate) {
        // Start up the PWM generator by copying from the mailbox
        pwmState.cnt              = 1;
        pwmState.idx              = 1;                           // Ensure copy this cycle (cause it to start at t = 0)
        pwmState.nextServiceCycle = NodeMCU::getCycleCountIRQ(); // Do the next service cycle it in this loop
        // No need for memory barrier here; global must be written by IRQ exit
    }

    if(wvfState.waveformEnabled || pwmState.cnt) {

        bool done = false;

        do {
            nextEventCycle = NodeMCU::getCycleCountIRQ() + NodeMCU::microsecondsToClockCycles(MaximumDelayBetweenIRQs);

            // PWM state machine implementation
            if(pwmState.cnt) {

                int32_t cyclesToGo;

                do {

                    cyclesToGo = pwmState.nextServiceCycle - NodeMCU::getCycleCountIRQ();

                    if(cyclesToGo < 0) {
                        if(pwmState.idx == pwmState.cnt) { // Start of pulses, possibly copy a new one
                            if(pwmState.pwmUpdate) {
                                // Do the memory copy from temporary to global and clear mailbox
                                pwmState = *pwmState.pwmUpdate;
                            }
                            GPOS = pwmState.mask; // Set all active pins high
                            if( pwmState.mask & (1 << 16) ) GP16O = 1;
                            pwmState.idx = 0;
                        }
                        else {
                            do {
                                // Drop the pin at this edge
                                if( pwmState.mask & (1 << pwmState.pin[pwmState.idx]) ) {
                                    GPOC = 1 << pwmState.pin[pwmState.idx];
                                    if(pwmState.pin[pwmState.idx] == 16) GP16O = 0;
                                }
                                ++pwmState.idx;
                                // Any other pins at this same PWM value will have zero delta, so drop them too
                            } while(pwmState.delta[pwmState.idx] == 0);
                        }
                        // Preserve duty cycle over PWM period by using (now + XXX) instead of (+= delta)
                        cyclesToGo                = ADJUST( pwmState.delta[pwmState.idx] );
                        pwmState.nextServiceCycle = NodeMCU::getCycleCountIRQ() + cyclesToGo;
                    }

                    nextEventCycle = earliest(nextEventCycle, pwmState.nextServiceCycle);

                } while( pwmState.cnt && (cyclesToGo < 100) );

            } // if(pwmState.cnt)

            for(uint16_t i = wvfState.startPin; i <= wvfState.endPin; ++i) {

                const uint32_t mask = 1 << i;

                // If it is not on, ignore it
                if( !(wvfState.waveformEnabled & mask) ) continue;

                Waveform* wave = &wvfState.waveform[i];

                const uint32_t now = NodeMCU::getCycleCountIRQ();

                // Disable any waveforms that are done
                if(wave->expiryCycle) {
                    const int32_t expiryToGo = wave->expiryCycle - now;
                    if(expiryToGo < 0) {
                        // Done, remove it
                        if(i == 16) GP16O = 0;
                        GPOC = mask;
                        wvfState.waveformEnabled &= ~mask;
                        continue;
                    }
                }

                // Check for toggles
                const int32_t cyclesToGo = wave->nextServiceCycle - now;

                if(cyclesToGo < 0) {

                    uint32_t* timeToUpdate;
                    uint32_t  nextEdgeCycles;
                    uint32_t  desired = 0;

                    wvfState.waveformState ^= mask;

                    if(wvfState.waveformState & mask) {
                        if(i == 16) GP16O = 1;
                        GPOS = mask;

                        if(wvfState.waveformToChange & mask) {
                            // Copy over next full-cycle timings
                            wave->timeHighCycles      = wvfState.waveformNewHigh;
                            wave->desiredHighCycles   = wvfState.waveformNewHigh;
                            wave->timeLowCycles       = wvfState.waveformNewLow;
                            wave->desiredLowCycles    = wvfState.waveformNewLow;
                            wave->lastEdge            = 0;
                            wvfState.waveformToChange = 0;
                        }
                        if(wave->lastEdge) {
                            desired      =  wave->desiredLowCycles;
                            timeToUpdate = &wave->timeLowCycles;
                        }
                        nextEdgeCycles = wave->timeHighCycles;
                    }
                    else {
                        if(i == 16) GP16O = 0;
                        GPOC = mask;
                        desired        =  wave->desiredHighCycles;
                        timeToUpdate   = &wave->timeHighCycles;
                        nextEdgeCycles =  wave->timeLowCycles;
                    }

                    if(desired) {
                        desired = ADJUST(desired);
                        int32_t err = desired - (now - wave->lastEdge);
                        if( static_cast<uint32_t>( eCxx::abs(err) ) < desired ) { // If the entire phase has been lost, ignore this error signal
                            err /= 2;
                            *timeToUpdate += err;
                        }
                    }

                    nextEdgeCycles         = ADJUST(nextEdgeCycles);
                    wave->nextServiceCycle = now + nextEdgeCycles;
                    wave->lastEdge         = now;

                } // if(cyclesToGo < 0)

                nextEventCycle = earliest(nextEventCycle, wave->nextServiceCycle);

            } // for(...)

            // Exit the loop if the fixed runtime limit has been exceeded or the next event is known to be
            // after that timeout would occur
            const uint32_t now                 = NodeMCU::getCycleCountIRQ();
            const int32_t  cycleDeltaNextEvent = nextEventCycle - now;
            const int32_t  cyclesLeftTimeout   = timeoutCycle - now;

            done = ( cycleDeltaNextEvent > static_cast<int32_t>(MINIRQTIME) ) || (cyclesLeftTimeout < 0);

        } while(!done);

    } // if(wvfState.waveformEnabled || pwmState.cnt)


    if(wvfState.timer1CB) {
        nextEventCycle = earliest( nextEventCycle, NodeMCU::getCycleCountIRQ() + wvfState.timer1CB() );
    }

    int32_t nextEventCycles = nextEventCycle - NodeMCU::getCycleCountIRQ();

    if( nextEventCycles <  static_cast<int32_t>(MINIRQTIME) ) nextEventCycles = MINIRQTIME;
    nextEventCycles -= DELTAIRQ;

    // Do it here instead of in the global function to save time and because it is an edge IRQ
    T1L = nextEventCycles >> (turbo ? 1 : 0);
}


//
// Timer1-related functions
//
static bool timer1Running = false;


static void initTimer1() __ECXX_ATTR_NOINLINE;
static void initTimer1()
{
    if(timer1Running) return;

    NodeMCU::timer1Disable();

    ETS_FRC_TIMER1_INTR_ATTACH(nullptr, nullptr);
    ETS_FRC_TIMER1_NMI_INTR_ATTACH(timer1Interrupt);

    NodeMCU::timer1Enable(TIM_DIV1, TIM_EDGE, TIM_SINGLE);
    timer1Running = true;
    NodeMCU::timer1Write( NodeMCU::microsecondsToClockCycles(10) );
}


static void ICACHE_RAM_ATTR forceTimer1Interrupt()
{ if( T1L > NodeMCU::microsecondsToClockCycles(10) ) T1L = NodeMCU::microsecondsToClockCycles(10); }


static void ICACHE_RAM_ATTR disableIdleTimer1()
{
    if(timer1Running && !wvfState.waveformEnabled && !pwmState.cnt && !wvfState.timer1CB) {
        ETS_FRC_TIMER1_NMI_INTR_ATTACH(nullptr);
        NodeMCU::timer1Disable();
        NodeMCU::timer1InitISR();
        timer1Running = false;
    }
}


/*
 * Add a callback function to be called on every timer1 trigger
 *     # The callback must return the number of CPU clock cycles until the next desired call.
 *     # However, since it is called every timer1 interrupt, it may be called again before this period.
 *     # It should therefore use the ESP Cycle Counter to determine whether or not to perform an operation.
 *     # Pass in nullptr to disable the callback and, if no other waveforms being generated, stop the timer as well.
 *     # Make sure the CB function has the ICACHE_RAM_ATTR decorator.
 */
extern void setWaveformTimer1Callback(uint32_t (*cb)())
{
    wvfState.timer1CB = cb;

    if(cb) {
        initTimer1();
        forceTimer1Interrupt();
    }

    disableIdleTimer1();
}


//
// Waveform-related functions
//
extern bool startWaveformClockCycles(uint8_t pin, uint32_t timeHighCycles, uint32_t timeLowCycles, uint32_t runTimeCycles)
{
    // Check the pin number
    if( !NodeMCU::userGPIOPin<false>(pin) ) return false;

    Waveform* wave = &wvfState.waveform[pin];

    wave->expiryCycle = runTimeCycles ? getCycleCount() + runTimeCycles : 0;

    if(runTimeCycles && !wave->expiryCycle) {
        // expiryCycle = 0 means no timeout, so avoid setting it
        wave->expiryCycle = 1;
    }

    // Make sure there is no active PWM here
    stopPWM(pin);

    const uint32_t mask = 1 << pin;

    MEMORY_BARRIER();

    if(wvfState.waveformEnabled & mask) {
        // Make sure no waveform changes are waiting to be applied
        while(wvfState.waveformToChange) {
            NodeMCU::delay(0); // Wait for waveform to update
            // No memory barrier here, the call to a global function implies global state updated
        }
        wvfState.waveformNewHigh  = timeHighCycles;
        wvfState.waveformNewLow   = timeLowCycles;
        MEMORY_BARRIER();
        wvfState.waveformToChange = mask;
        // The waveform will be updated some time in the future on the next period for the signal
    }
    else {
        wave->timeHighCycles       = timeHighCycles;
        wave->desiredHighCycles    = timeHighCycles;
        wave->timeLowCycles        = timeLowCycles;
        wave->desiredLowCycles     = timeLowCycles;
        wave->lastEdge             = 0;
        wave->nextServiceCycle     = NodeMCU::getCycleCount() + NodeMCU::microsecondsToClockCycles(1);
        wvfState.waveformToEnable |= mask;
        MEMORY_BARRIER();
        initTimer1();
        forceTimer1Interrupt();
        while(wvfState.waveformToEnable) {
            NodeMCU::delay(0); // Wait for waveform to update
            // No memory barrier here, the call to a global function implies global state updated
        }
    }

    return true;
}


extern bool startWaveform(uint8_t pin, uint32_t timeHighUS, uint32_t timeLowUS, uint32_t runTimeUS)
{
    return startWaveformClockCycles(
        pin,
        NodeMCU::microsecondsToClockCycles(timeHighUS),
        NodeMCU::microsecondsToClockCycles(timeLowUS ),
        NodeMCU::microsecondsToClockCycles(runTimeUS )
    );
}


extern bool ICACHE_RAM_ATTR stopWaveform(uint8_t pin)
{
    // Check if there is no timer active
    if(!timer1Running) return false;

    const uint32_t mask = 1 << pin;

    if (wvfState.waveformEnabled & mask) {
        wvfState.waveformToDisable = mask;
        // Cancel any pending updates for this waveform
        if(wvfState.waveformToChange & mask) {
            wvfState.waveformToChange = 0;
        }
        forceTimer1Interrupt();
        while(wvfState.waveformToDisable) {
            MEMORY_BARRIER(); // If it was not written yet, it has to be by now
            // Can not delay() here because stopWaveform() may be called from an IRQ
        }
    }

    disableIdleTimer1();

    return true;
}


//
// PWM-related functions
//
static void ICACHE_RAM_ATTR notifyPWM(PWMState *p, bool idle)
{
    // # Notify the NMI that a new PWM state is available through the mailbox
    // # Wait for mailbox to be emptied, either busy or delay() as needed

    p->pwmUpdate = nullptr;
    pwmState.pwmUpdate = p;
    MEMORY_BARRIER();

    forceTimer1Interrupt();

    while(pwmState.pwmUpdate) {
        if(idle) NodeMCU::delay(0);
        MEMORY_BARRIER();
    }
}


static void addPWMtoList(PWMState& p, int8_t pin, uint32_t val, uint32_t range)
{
    // # Stash the value and range so that the fraction can be reevaluated should the user change the PWM frequency
    // # This allows the algorithm to give as much precision as possible
    // # Because by construction the waveform for this pin will be inactive, borrow the memory from that structure

    wvfState.waveform[pin].desiredHighCycles = val;   // Numerator   -> high
    wvfState.waveform[pin].desiredLowCycles  = range; // Denominator -> low

    uint32_t cc = (pwmPeriod * val) / range;

    // Clip the values, in the case of adjusting frequencies makes the values to become out of range
         if(cc == 0        ) cc = 1;
    else if(cc >= pwmPeriod) cc = pwmPeriod - 1;

    if(p.cnt == 0) {
        // Starting up from scratch, special case 1st element and PWM period
        p.pin  [0] = pin;
        p.delta[0] = cc;
        // Final pin is never used : p.pin[1] = 0xFF;
        p.delta[1] = pwmPeriod - cc;
    }

    else {
        uint32_t ttl = 0;
        uint32_t i;
        // Skip along to the spot to insert
        for( i = 0; (i <= p.cnt) && (ttl + p.delta[i] < cc); ++i ) {
            ttl += p.delta[i];
        }
        // Shift everything out by one to make space for the new edge
        for( int32_t j = p.cnt; j >= static_cast<int32_t>(i); --j ) {
            p.pin  [j + 1] = p.pin  [j];
            p.delta[j + 1] = p.delta[j];
        }
        const int32_t off = cc - ttl; // The delta from the last edge to the one being inserted
        p.pin  [i    ]  = pin;
        p.delta[i    ]  = off; // Add the delta to this new pin
        p.delta[i + 1] -= off; // And subtract it from the follower to keep sum(deltas) constant
    }

    ++p.cnt;
    p.mask |= (1 << pin);
}


static void cleanAndRemovePWM(PWMState* p, int8_t pin)
{
    // Helper routine to remove an entry from the state machine and clean up any marked-off entries

    uint32_t leftover = 0;
    uint32_t in;
    uint32_t out;

    for(in = 0, out = 0; in < p->cnt; ++in) {
        if( ( p->pin[in] != pin ) && ( p->mask & ( 1 << p->pin[in] ) ) ) {
            p->pin  [out] = p->pin  [in];
            p->delta[out] = p->delta[in] + leftover;
            leftover = 0;
            ++out;
        }
        else {
            leftover +=         p->delta[in]  ;
            p->mask  &= ~( 1 << p->pin  [in] );
        }
    }
    p->cnt = out;

    // Final pin is never used : p->pin[out] = 0xFF;
    p->delta[out] = p->delta[in] + leftover;
}


extern void setPWMFrequency(uint32_t freq)
{
    pwmFrequency = freq;

    // Convert frequency into clock cycles
    uint32_t cc = NodeMCU::microsecondsToClockCycles(1000000UL) / freq;

    // Simple static adjustment to bring the period closer to the requested due to overhead;
    // empirically determined as a constant PWM delay and a function of the number of PWMs
#if F_CPU == 80000000
    cc -= ( (NodeMCU::microsecondsToClockCycles(pwmState.cnt) * 13) >> 4 ) + 110;
#else
    cc -= ( (NodeMCU::microsecondsToClockCycles(pwmState.cnt) * 10) >> 4 ) +  75;
#endif

    if(cc == pwmPeriod) return;

    pwmPeriod = cc;

    if(pwmState.cnt) {
        PWMState p; // Use a working copy because the one in use cannot be modified
        p.mask = 0;
        p.cnt  = 0;
        for(uint32_t i = 0; i < pwmState.cnt; ++i) {
            const uint8_t pin = pwmState.pin[i];
            addPWMtoList(p, pin, wvfState.waveform[pin].desiredHighCycles, wvfState.waveform[pin].desiredLowCycles);
        }
        // Update and wait for mailbox to be emptied
        initTimer1();
        notifyPWM(&p, true);
        disableIdleTimer1();
    }
}


extern bool startPWM(int8_t pin, uint32_t val, uint32_t range)
{
    // Check the pin number
    if( !NodeMCU::userGPIOPin<false>(pin) ) return false;

    // Make sure there is no active PWM here
    stopWaveform(pin);

    PWMState p = pwmState; // Use a working copy because the one in use cannot be modified

    // Get rid of any entries for this pin
    cleanAndRemovePWM(&p, pin);

    // Check if there is no space left
    if(p.cnt >= MAX_PWM) return false;

    // Sanity check for all on/off
    const uint32_t cc = (pwmPeriod * val) / range;

    if( (cc == 0) || (cc >= pwmPeriod) ) {
        digitalWrite(pin, cc ? true : false);
        return true;
    }

    // Add it to the list
    addPWMtoList(p, pin, val, range);

    // Set mailbox and wait for ISR to copy it over
    initTimer1();
    notifyPWM(&p, true);
    disableIdleTimer1();

    // Potentially recalculate the PWM period if another pin has been added
    setPWMFrequency(pwmFrequency);

    return true;
}


extern bool ICACHE_RAM_ATTR stopPWM(int8_t pin)
{
    // Disable PWM on a specific pin, i.e. when analogWrite(0%/100%)

    if( !( (1 << pin) & pwmState.mask ) ) {
        return false; // Pin is not actually active
    }

    PWMState p = pwmState; // Use a working copy because the one in use cannot be modified

    // # Simply clear the mask but keep everything else untouched to save IRAM
    // # The main startPWM() will handle cleanup
    p.mask &= ~(1 << pin);

    if(!p.mask) {
        // If all have been stopped, then turn PWM off completely
        p.cnt = 0;
    }

    // Update and wait for mailbox to be emptied, no delay (could be in ISR)
    notifyPWM(&p, false);

    // Shut down the timer completely if there is no more active PWM
    disableIdleTimer1();

    return true;
}


} // namespace NodeMCU

} // namespace eCxx
