#include "LookupTable.h"
#include "Motor.h"

///* External inputs (root inport signals with auto storage) */
//xdata ExtU rtU;

///* External outputs (root outports fed by signals with auto storage) */
//xdata ExtY rtY;

//xdata const ConstP rtConstP = {
//  { 1000, 2000, 3000, 4000, 5000, 6000 }, // uDLookupTable_tableData[]
//  { 100, 200, 300, 400, 500, 600 } // uDLookupTable_bp01Data[]
//};

#if ((Vsp_LookUpTable == 1) || (Power_LookUpTable == 1))
int look1_binlx(int u0, const int bp0[], const int table[], unsigned char maxIndex);

int look1_binlx(int u0, const int bp0[], const int table[], unsigned char maxIndex)
{
  //float frac;
  unsigned char iRght;
  unsigned char iLeft;
  unsigned char bpIdx;
  int Out;

  if (u0 <= bp0[0]) {
    iLeft = 0;
    //frac = ((float) (u0 - bp0[0]) / (bp0[1] - bp0[0]));
  } else if (u0 < bp0[maxIndex]) {
    
    /* Binary Search */
    bpIdx = maxIndex >> 1;
    iLeft = 0;
    iRght = maxIndex;
    while ((iRght - iLeft) > 1) {
      if (u0 < bp0[bpIdx]) {
        iRght = bpIdx;
      } else {
        iLeft = bpIdx;
      }
      bpIdx = (iRght + iLeft) >> 1;
    }

    //frac = ((float) (u0 - bp0[iLeft]) / (bp0[iLeft + 1] - bp0[iLeft]));
  } else {
    iLeft = maxIndex - 1;
    //frac = ((float) (u0 - bp0[maxIndex - 1]) / (bp0[maxIndex] - bp0[maxIndex - 1]));
  }

  Out = (int)((long) (table[iLeft + 1] - table[iLeft]) * (u0 - bp0[iLeft]) / (bp0[iLeft+1] - bp0[iLeft]) + table[iLeft]);
  return Out;
}
#endif 

///* Model step function */
//void Subsystem_step(void)
//{
//    rtU.In1 = 550;
//    rtY.Out1 = look1_binlx(rtU.In1, rtConstP.uDLookupTable_IntputData, rtConstP.uDLookupTable_tableData, 5);
//}

