/*
Speeduino - Simple engine management for the Arduino Mega 2560 platform
Copyright (C) Josh Stewart
A full copy of the license may be found in the projects root directory
*/

/*
Because the size of the table is dynamic, this function is required to reallocate the array sizes
Note that this may clear some of the existing values of the table
*/
#include "table2d.h"
#if !defined(UNIT_TEST)
#include "globals.h"
#endif

static void construct2dTable(table2D &table, uint8_t valueSize, uint8_t axisSize, uint8_t length, void *values, void *bins) {
  table.valueSize = valueSize;
  table.axisSize = axisSize;
  table.xSize = length;
  table.values = values;
  table.axisX = bins;
  table.lastInput = INT16_MAX;
  table.lastXMax = INT16_MAX;
  table.lastXMin = INT16_MAX;
}

void construct2dTable(table2D &table, uint8_t length, uint8_t *values, uint8_t *bins) {
  construct2dTable(table, SIZE_BYTE, SIZE_BYTE, length, values, bins);
}
void construct2dTable(table2D &table, uint8_t length, uint8_t *values, int8_t *bins) {
  construct2dTable(table, SIZE_BYTE, SIZE_SIGNED_BYTE, length, values, bins);
}
void construct2dTable(table2D &table, uint8_t length, uint16_t *values, uint16_t *bins) {
  construct2dTable(table, SIZE_INT, SIZE_INT, length, values, bins);
}
void construct2dTable(table2D &table, uint8_t length, uint8_t *values, uint16_t *bins) {
  construct2dTable(table, SIZE_BYTE, SIZE_INT, length, values, bins);
}
void construct2dTable(table2D &table, uint8_t length, uint16_t *values, uint8_t *bins) {
  construct2dTable(table, SIZE_INT, SIZE_BYTE, length, values, bins);
}
void construct2dTable(table2D &table, uint8_t length, int16_t *values, uint8_t *bins) {
  construct2dTable(table, SIZE_INT, SIZE_BYTE, length, values, bins);
}

static inline uint8_t getCacheTime(void) {
#if !defined(UNIT_TEST)
  return currentStatus.secl;
#else
  return 0;
#endif
}

/*
This function pulls a 1D linear interpolated (ie averaged) value from a 2D table
ie: Given a value on the X axis, it returns a Y value that corresponds to the point on the curve between the nearest two defined X values

This function must take into account whether a table contains 8-bit or 16-bit values.
Unfortunately this means many of the lines are duplicated depending on this
*/
int table2D_getValue(struct table2D *fromTable, int X_in)
{
  //Orig memory usage = 5414
  int returnValue = 0;
  bool valueFound = false;

  int X = X_in;
  int xMinValue, xMaxValue;
  int xMin = 0;
  int xMax = fromTable->xSize-1;

  //Check whether the X input is the same as last time this ran
  if( (X_in == fromTable->lastInput) && (fromTable->cacheTime == getCacheTime()) )
  {
    returnValue = fromTable->lastOutput;
    valueFound = true;
  }
  //If the requested X value is greater/small than the maximum/minimum bin, simply return that value
  else if(X >= table2D_getAxisValue(fromTable, xMax))
  {
    returnValue = table2D_getRawValue(fromTable, xMax);
    valueFound = true;
  }
  else if(X <= table2D_getAxisValue(fromTable, xMin))
  {
    returnValue = table2D_getRawValue(fromTable, xMin);
    valueFound = true;
  }
  //Finally if none of that is found
  else
  {
    fromTable->cacheTime = getCacheTime(); //As we're not using the cache value, set the current secl value to track when this new value was calculated

    //1st check is whether we're still in the same X bin as last time
    xMaxValue = table2D_getAxisValue(fromTable, fromTable->lastXMax);
    xMinValue = table2D_getAxisValue(fromTable, fromTable->lastXMin);
    if ( (X <= xMaxValue) && (X > xMinValue) )
    {
      xMax = fromTable->lastXMax;
      xMin = fromTable->lastXMin;
    }
    else
    {
      //If we're not in the same bin, loop through to find where we are
      xMaxValue = table2D_getAxisValue(fromTable, fromTable->xSize-1); // init xMaxValue in preparation for loop.
      for (int x = fromTable->xSize-1; x > 0; x--)
      {
        xMinValue = table2D_getAxisValue(fromTable, x-1); // fetch next Min

        //Checks the case where the X value is exactly what was requested
        if (X == xMaxValue)
        {
          returnValue = table2D_getRawValue(fromTable, x); //Simply return the corresponding value
          valueFound = true;
          break;
        }
        else if (X > xMinValue)
        {
          // Value is in the current bin
          xMax = x;
          fromTable->lastXMax = xMax;
          xMin = x-1;
          fromTable->lastXMin = xMin;
          break;
        }
        // Otherwise, continue to next bin
        xMaxValue = xMinValue; // for the next bin, our Min is their Max
      }
    }
  } //X_in same as last time

  if (valueFound == false)
  {
    int16_t m = X - xMinValue;
    int16_t n = xMaxValue - xMinValue;

    int16_t yMax = table2D_getRawValue(fromTable, xMax);
    int16_t yMin = table2D_getRawValue(fromTable, xMin);

    /* Float version (if m, yMax, yMin and n were float's)
       int yVal = (m * (yMax - yMin)) / n;
    */
    
    //Non-Float version
    int16_t yVal = ( ((int32_t) m) * (yMax-yMin) ) / n;
    returnValue = yMin + yVal;
  }

  fromTable->lastInput = X_in;
  fromTable->lastOutput = returnValue;

  return returnValue;
}

/**
 * @brief Returns an axis (bin) value from the 2D table. This works regardless of whether that axis is bytes or int16_ts
 * 
 * @param fromTable 
 * @param X_in 
 * @return int16_t 
 */
int16_t table2D_getAxisValue(struct table2D *fromTable, byte X_in)
{
  int returnValue = 0;

  if(fromTable->axisSize == SIZE_INT) { returnValue = ((int16_t*)fromTable->axisX)[X_in]; }
  else if(fromTable->axisSize == SIZE_BYTE) { returnValue = ((uint8_t*)fromTable->axisX)[X_in]; }
  else if(fromTable->axisSize == SIZE_SIGNED_BYTE) { returnValue = ((int8_t*)fromTable->axisX)[X_in]; }
  

  return returnValue;
}

/**
 * @brief Returns an value from the 2D table given an index value. No interpolation is performed
 * 
 * @param fromTable 
 * @param X_index 
 * @return int16_t 
 */
int16_t table2D_getRawValue(struct table2D *fromTable, byte X_index)
{
  int returnValue = 0;

  if(fromTable->valueSize == SIZE_INT) { returnValue = ((int16_t*)fromTable->values)[X_index]; }
  else if(fromTable->valueSize == SIZE_BYTE) { returnValue = ((uint8_t*)fromTable->values)[X_index]; }
  else if(fromTable->valueSize == SIZE_SIGNED_BYTE) { returnValue = ((int8_t*)fromTable->values)[X_index]; }

  return returnValue;
}