#include "./stairs.h"
#include <Math.h>
#include <FS.h>
#include "./debug.h"
#include "./global.h"



const static float CurveFactor = log10(2) / log10(4095);
const static float LinearFactor = 4095.0f / 255.0f;


void Stairs::init(PCA9685* pwmDriver)
{
  mPWMDriver = pwmDriver;

  memset(&mStep[0], 0, sizeof(mStep));
}


uint8_t Stairs::ease(uint8_t startValue, uint8_t targetValue, uint16_t transitionTime, uint16_t elapsedTime)
{
  bool up = targetValue > startValue;

  uint16_t diff = up ? targetValue - startValue : startValue - targetValue;
  uint16_t delta = (diff * elapsedTime) / transitionTime;

  int16_t currentValue = up ? startValue + delta : startValue - delta;
  if (currentValue < 0) currentValue = 0;
  if (currentValue > 255) currentValue = 255;

  return currentValue;
}


inline void Stairs::updateCurrentValue(Step* stepState)
{
  int32_t stepElapsedTime = -stepState->startTime;
  stepState->currentValue = ease(stepState->startValue, stepState->targetValue, stepState->remainingTime + stepElapsedTime, stepElapsedTime);
}


inline void Stairs::applyCurrentValue(uint8_t step)
{
  mPWMDriver->setPWM(step, this->getPWMValue(step, mStep[step].currentValue));
}


void Stairs::tick()
{
  if (stepsSettingsChanged)
  {
    // Re-apply all values in case the PWM value changed
    for (uint8_t step = 0; step < stepsSettings->count(); step++)
      applyCurrentValue(step);

    stepsSettingsChanged = false;
  }


  if (!mTick) return;

  uint32_t elapsedTime = mLastTransitionTime != 0 ? currentTime - mLastTransitionTime : 0;
  if (!elapsedTime) return;


  mLastTransitionTime = currentTime;
  mTick = false;


  for (uint8_t step = 0; step < stepsSettings->count(); step++)
  {
    Step* stepState = &mStep[step];

    if (stepState->currentValue != stepState->targetValue)
    {
      // If there is a startup delay request, wait for it first
      if (stepState->startTime > 0)
      {
        stepState->startTime -= elapsedTime;
        if (stepState->startTime < 0)
        {
          if (stepState->remainingTime > -stepState->startTime)
          {
            // Shift the remaining time equally
            stepState->remainingTime += stepState->startTime;
            updateCurrentValue(stepState);
            mTick = true;
          }
          else
          {
            // End of the transition
            stepState->remainingTime = 0;
            stepState->currentValue = stepState->targetValue;
          }

          applyCurrentValue(step);
        }
        else
          mTick = true;
      }
      else if (elapsedTime >= stepState->remainingTime)
      {
        // End of the transition
        stepState->remainingTime = 0;
        stepState->currentValue = stepState->targetValue;

        applyCurrentValue(step);
      }
      else
      {
        stepState->startTime -= elapsedTime;
        stepState->remainingTime -= elapsedTime;

        updateCurrentValue(stepState);
        applyCurrentValue(step);

        mTick = true;
      }
    }
  }

  if (!mTick)
    mLastTransitionTime = 0;
}


uint8_t Stairs::get(uint8_t step, bool target)
{
  if (step >= MaxStepCount) return 0;
  return target ? mStep[step].targetValue : mStep[step].currentValue;
}


void Stairs::set(uint8_t step, uint8_t brightness, uint16_t transitionTime, uint16_t startTime)
{
  _d("Stairs :: set step = "); _d(step);
  _d(", brightness = "); _d(brightness);
  _d(", transitionTime = "); _d(transitionTime);
  _d(", startTime = "); _dln(startTime);


  if (step >= MaxStepCount) return;
  if (mStep[step].currentValue == brightness)
    return;

  mStep[step].targetValue = brightness;

  if (transitionTime > 0)
  {
    mStep[step].startValue = mStep[step].currentValue;
    mStep[step].startTime = startTime;
    mStep[step].remainingTime = transitionTime;

    if (!mLastTransitionTime)
      mLastTransitionTime = currentTime;

    mTick = true;
  }
  else
  {
    mStep[step].currentValue = brightness;
    applyCurrentValue(step);
  }
}


void Stairs::setAll(uint8_t brightness, uint16_t transitionTime, uint16_t startTime)
{
  for (uint8_t step = 0; step < stepsSettings->count(); step++)
    set(step, brightness, transitionTime, startTime);
}


void Stairs::sweep(uint8_t brightness, uint16_t transitionTime, bool topDown)
{
  uint8_t stepsCount = stepsSettings->count();
  uint16_t offsetIncrement = stepsCount > 0 ? (transitionTime / stepsCount) * 1.5 : 0;
  uint16_t offset = topDown ? 0 : (stepsCount - 1) * offsetIncrement;

  for (uint8_t step = 0; step < stepsCount; step++)
  {
    set(step, brightness, transitionTime, offset);

    if (topDown)
      offset += offsetIncrement;
    else
      offset -= offsetIncrement;
  }
}


uint16_t Stairs::getPWMValue(uint8_t step, uint8_t brightness)
{
  //_d("Stairs :: Getting PWM value for step "); _d(step); _d(", brightness "); _dln(brightness);
  if (brightness == 0 || brightness == 255)
  {
    //_dln("Stairs :: Full on/off, returning input");
    return brightness == 0 ? 0 : 4095;
  }

  uint16_t pwmValue;
  uint16_t rangeStart = stepsSettings->rangeStart(step);
  uint16_t rangeEnd = stepsSettings->rangeEnd(step);

  if (stepsSettings->useCurve())
  {
    //_dln("Stairs :: Using curve");
    float factor = ((rangeEnd - rangeStart) + 1) * CurveFactor;
    pwmValue = pow(2, ((brightness * LinearFactor) / factor)) - 1 + rangeStart;
  }
  else
  {
    //_dln("Stairs :: Not using curve");
    float factor = ((rangeEnd - rangeStart) + 1) / 255.0f;
    pwmValue = (brightness * factor) + rangeStart;
  }

  //_d("Stairs :: Output: "); _dln(pwmValue);
  return pwmValue;
}