#define ADC_Range_Pin 35  // Pin Range Sensor

//***************************************************************************
//**                       Range Sensor Instanzen
//***************************************************************************
long general_t;

float Current_range = 0;    // 4-20 mA
float ADCVoltage_range = 0; // 0-3,3 V
float range_reading = 0;    // 0-50mm
int round_range;

//** Matematika mapfloat ----------------------------------
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//** Matematika movingAverage -----------------------------
float movingAverage(float value)
{
  const byte nvalues = 16; // Moving average window size
  static byte current = 0; // Index for current value
  static byte cvalues = 0; // Count of values read (<= nvalues)
  static float sum = 0;    // Rolling sum
  static float values[nvalues];

  sum += value;

  //** If the window is full, adjust the sum by deleting the oldest value
  if (cvalues == nvalues)
    sum -= values[current];

  values[current] = value; // Replace the oldest with the latest

  if (++current >= nvalues)
    current = 0;

  if (cvalues < nvalues)
    cvalues += 1;

  return sum / cvalues;
}

//***********************************************************************************
//**                               void setup()
//***********************************************************************************
void setup() {
  Serial.begin(115200);
}

//***********************************************************************************
//**                                 void loop()
//***********************************************************************************
void loop() {

  Read_Range_Sensor();

  if (millis() - general_t > 500)
  {
    //Serial.println(ReadVoltage(ADC_Range_Pin), 3);
    //Serial.println(analogRead(ADC_Range_Pin));
    Serial.println(range_reading);
    Serial.println(round_range);
    general_t = millis();
  }
}

//***********************************************************************************
//**                       double ReadVoltage(byte pin)
//***********************************************************************************
double ReadVoltage(byte pin) {
  double reading = analogRead(pin); // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
  if (reading < 1 || reading > 4095) return 0;
  // return -0.000000000009824 * pow(reading,3) + 0.000000016557283 * pow(reading,2) + 0.000854596860691 * reading + 0.065440348345433;
  return -0.000000000000016 * pow(reading, 4) + 0.000000000118171 * pow(reading, 3) - 0.000000301211691 * pow(reading, 2) + 0.001109019271794 * reading + 0.034143524634089;
} // Added an improved polynomial, use either, comment out as required


//***********************************************************************************
//**                       void Read_Range_Sensor()
//***********************************************************************************
void Read_Range_Sensor()
{
  ADCVoltage_range = (float)(movingAverage(ReadVoltage(ADC_Range_Pin)));

  // For 160 Ohm: 0.004 A * 160 Ohm = 660 mV, 0.02 A * 160 Ohm = 3300 mV
  // Voltage between 660-3300 mV is distributed as current 4-20 mA
  Current_range = mapfloat(ADCVoltage_range, 0.0, 3.14, 4, 20);
  // Current between 4-20 mA is distributed as distance between 0-50 mm.
  range_reading = mapfloat(Current_range, 4, 20, 0, 50);

  // cil_back and cil_front is to simulate position sensors on the cilinder
  round_range = int(range_reading + 0.5); // Rounding the range from float to int to use as position sensor of the cylinder
}