#include "IndSensorLUT.hpp"
#include <Arduino.h>
#include <avr/pgmspace.h>
#include "ADC.hpp"

// LUT active range: 4.0..18.0 mm at 0.1 mm resolution (141 entries each).
// Generated from A0Calibration/data/Sensor{0,1,2,5}.xlsx via linear interpolation
// onto a uniform mm grid; strictly monotonic increasing in ADC.

static const uint16_t ind0LUT[141] PROGMEM = {
    67,   71,   74,   77,   81,   85,   99,  102,  108,  113,
   117,  120,  125,  135,  141,  151,  154,  158,  165,  170,
   181,  194,  196,  207,  212,  218,  223,  233,  234,  250,
   265,  267,  279,  284,  291,  296,  301,  309,  327,  334,
   346,  350,  357,  360,  366,  375,  379,  387,  411,  421,
   422,  429,  434,  441,  451,  462,  470,  480,  487,  488,
   497,  502,  505,  515,  523,  530,  537,  545,  546,  559,
   566,  570,  575,  587,  592,  596,  601,  612,  618,  625,
   628,  632,  636,  640,  646,  653,  658,  663,  668,  672,
   678,  684,  686,  688,  691,  699,  703,  708,  709,  711,
   715,  719,  722,  726,  729,  733,  737,  739,  743,  747,
   748,  751,  754,  759,  760,  763,  766,  769,  770,  772,
   775,  776,  779,  781,  783,  785,  788,  791,  792,  794,
   796,  797,  798,  800,  802,  803,  806,  807,  808,  810,
   811
};

static const uint16_t ind1LUT[141] PROGMEM = {
    64,   68,   72,   77,   80,   87,   93,   96,  100,  109,
   114,  121,  127,  133,  139,  146,  152,  164,  169,  176,
   183,  192,  200,  207,  217,  224,  234,  242,  251,  259,
   270,  280,  289,  299,  310,  319,  329,  337,  348,  358,
   363,  377,  386,  395,  407,  416,  428,  437,  445,  453,
   462,  475,  485,  497,  505,  515,  527,  532,  547,  553,
   564,  573,  582,  591,  603,  611,  619,  626,  634,  644,
   653,  658,  667,  675,  683,  691,  700,  704,  713,  721,
   728,  735,  742,  748,  755,  762,  768,  774,  781,  786,
   793,  798,  804,  809,  815,  820,  826,  830,  835,  840,
   844,  849,  854,  859,  863,  867,  870,  874,  878,  881,
   887,  890,  894,  896,  899,  902,  905,  909,  911,  914,
   916,  921,  923,  926,  929,  932,  934,  937,  940,  942,
   944,  946,  949,  951,  953,  955,  957,  959,  961,  962,
   965
};

static const uint16_t ind2LUT[141] PROGMEM = {
    58,   60,   65,   73,   76,   79,   85,   90,   94,   97,
   101,  105,  114,  121,  126,  131,  136,  142,  148,  153,
   163,  172,  178,  183,  188,  194,  202,  216,  226,  231,
   241,  246,  253,  263,  271,  276,  284,  293,  306,  313,
   318,  326,  337,  345,  353,  361,  369,  387,  394,  400,
   406,  413,  420,  426,  434,  443,  450,  462,  467,  468,
   475,  481,  486,  494,  501,  510,  518,  525,  531,  536,
   542,  546,  556,  561,  567,  572,  577,  582,  588,  594,
   599,  606,  612,  615,  623,  627,  631,  634,  638,  641,
   646,  654,  657,  660,  664,  668,  672,  675,  679,  684,
   687,  691,  694,  698,  701,  704,  706,  708,  711,  714,
   716,  719,  722,  725,  727,  728,  732,  735,  737,  739,
   740,  743,  745,  746,  748,  750,  751,  752,  755,  758,
   760,  761,  762,  764,  765,  768,  769,  770,  771,  772,
   773
};

static const uint16_t ind5LUT[141] PROGMEM = {
    44,   47,   50,   54,   59,   62,   64,   69,   72,   77,
    81,   86,   90,   97,  101,  104,  113,  118,  122,  127,
   132,  139,  147,  154,  159,  167,  175,  180,  188,  193,
   201,  212,  222,  228,  234,  239,  253,  258,  265,  273,
   285,  292,  300,  307,  315,  323,  335,  349,  355,  364,
   371,  375,  384,  392,  399,  407,  418,  424,  433,  440,
   448,  455,  464,  472,  482,  488,  494,  501,  509,  520,
   529,  536,  541,  548,  554,  558,  564,  574,  579,  586,
   591,  598,  607,  611,  617,  624,  627,  632,  638,  646,
   652,  657,  661,  665,  670,  675,  682,  687,  691,  695,
   698,  703,  709,  712,  716,  719,  722,  728,  731,  735,
   739,  743,  747,  749,  752,  754,  758,  761,  764,  768,
   770,  773,  775,  777,  781,  784,  787,  789,  791,  792,
   795,  797,  799,  801,  803,  805,  807,  808,  810,  813,
   814
};

const IndSensorLUT ind0LUTCal = { ind0LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind1LUTCal = { ind1LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind2LUTCal = { ind2LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind5LUTCal = { ind5LUT, 141, 4.0f, 0.1f };

IndSensorL::IndSensorL(IndSensorLUT calibration, uint8_t analogPin, float emaAlpha)
  : oor(false), oorDir(0), mmVal(0.0f), analog(0), alpha(emaAlpha),
    cal(calibration), pin(analogPin), filteredRaw(0.0f) {
  filteredRaw = analogRead(pin);
}

// Binary search the monotonic ADC table for `raw`, then linearly interpolate mm.
// LUT lives in Flash (PROGMEM); all reads go through pgm_read_word().
// Sets oor + oorDir when raw falls outside the calibrated range.
float IndSensorL::toMM(uint16_t raw) {
  const uint16_t* t = cal.adcTable;
  const uint16_t n = cal.length;

  uint16_t t0   = pgm_read_word(&t[0]);
  uint16_t tEnd = pgm_read_word(&t[n - 1]);

  if (raw <= t0) {
    oor = true;
    oorDir = -1;  // ADC below table → gap below mmMin → too close
    return cal.mmMin;
  }
  if (raw >= tEnd) {
    oor = true;
    oorDir = +1;  // ADC above table → gap above mmMax → too far
    return cal.mmMin + (n - 1) * cal.mmStep;
  }

  uint16_t lo = 0, hi = n - 1;
  while (hi - lo > 1) {
    uint16_t mid = (lo + hi) >> 1;
    if (pgm_read_word(&t[mid]) <= raw) lo = mid;
    else                               hi = mid;
  }

  uint16_t vLo = pgm_read_word(&t[lo]);
  uint16_t vHi = pgm_read_word(&t[hi]);
  float frac = (float)(raw - vLo) / (float)(vHi - vLo);
  return cal.mmMin + ((float)lo + frac) * cal.mmStep;
}

float IndSensorL::readMM() {
  uint8_t index = pin - A0;
  index = (index > 3) ? index - 2 : index;
  uint16_t raw;

  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    raw = adc_results[index];
  }

  filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;
  analog = (uint16_t)filteredRaw;

  oor = false;
  oorDir = 0;
  mmVal = toMM(analog);
  return mmVal;
}

// Face → sensor pin/LUT mapping (assigned by user).
IndSensorL indF(ind2LUTCal, A5);
IndSensorL indL(ind1LUTCal, A1);
IndSensorL indR(ind0LUTCal, A0);
IndSensorL indB(ind5LUTCal, A4);