/**
 * @file    Interpolation_FlexiForce_Estimation.ino
 * @author  Charles STELANDRE
 * @version V0.1
 * @date    2025/05/11
 * @brief   Description: this code estimates the mass from a FlexiForce A201 sensor.
 *          First, you must record the signal within a range of [0-5]V, and the mass put on the sensor
 *          Second, you must report this data in the calibrationData. You must do so in an ascending order.
 *          Test the full range. Masses outside of the range you tested may not give the required result.
 *          
 *
 * Function List:
 * 1. double getVoltage(int rawValue)
 * 2. double getMass(double voltage)
 * 3. void setup()
 * 4. void loop()
 */
 
#include <Servo.h>

Servo myServo;  // Create a servo object
const int sensorPin = A0; // Analog pin for the weight sensor
const int servoPin = 9;  // Digital pin for the servo

// Calibration data: {voltage_in_V, mass_in_grams}
//Enter your calibration data here.
double calibrationData[][2] = {
  {1.776, 500},
  {3.288, 200},
  {4.076, 100},
  {4.530, 50},
  {4.824, 20},
  {4.924, 10},
  {4.92, 0},
};
const int calibrationDataSize = sizeof(calibrationData) / sizeof(calibrationData[0]);

// Function to convert raw analog reading (0-1023) to voltage (0-5V)
double getVoltage(int rawValue) {
  return (rawValue * 5.0) / 1023.0;
}

// Function to map the sensor voltage to mass in grams using linear interpolation
double getMass(double voltage) {
  if (voltage <= calibrationData[0][0]) {
    return calibrationData[0][1];
  }
  if (voltage >= calibrationData[calibrationDataSize - 1][0]) {
    return calibrationData[calibrationDataSize - 1][1];
  }

  for (int i = 0; i < calibrationDataSize - 1; i++) {
    if (voltage >= calibrationData[i][0] && voltage < calibrationData[i + 1][0]) {
      return calibrationData[i][1] + (voltage - calibrationData[i][0]) *
             (calibrationData[i + 1][1] - calibrationData[i][1]) /
             (calibrationData[i + 1][0] - calibrationData[i][0]);
    }
  }
  return -1; // Should not happen if data is sorted
}

// Function to map the mass (0-500g initially, scaling to 5kg later) to servo angle (0-180 degrees)
int massToAngle(double mass) {
  // Assuming the servo has a 180-degree range (can increase to show when out of range).
  // We'll map the current tested range (0-500g) to the full servo range.
  // Once you have data up to 5kg, you might need to adjust this mapping.
  return map(mass, 0, 500, 0, 180); // Change 0 and 500 to your calibration points' limits if needed
}

void setup() {
  Serial.begin(9600);
  myServo.attach(servoPin); // Attaches the servo on pin 9 to the servo object
  Serial.println("Weight Sensor Test Sketch (Voltage Input)");
}

void loop() {
  int rawValue = analogRead(sensorPin);
  double currentVoltage = getVoltage(rawValue);
  double currentMass = getMass(currentVoltage);

  Serial.print("Raw Sensor Value: ");
  Serial.print(rawValue);
  Serial.print(" | Voltage: ");
  Serial.print(currentVoltage);
  Serial.print(" V | Estimated Mass: ");
  Serial.print(currentMass);
  Serial.println(" g");

  if (currentMass != -1) {
    int servoAngle = massToAngle(currentMass);
    servoAngle = constrain(servoAngle, 0, 180); // Ensure the angle is within the servo's limits
    myServo.write(servoAngle);
    Serial.print("Servo Angle: ");
    Serial.println(servoAngle);
  } else {
    Serial.println("Error: Sensor voltage out of calibration range.");
  }

  delay(100); // Adjust delay as needed
}