/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: checkObstacle
	- Source Code compiled for: Arduino Nano
	- Source Code created on: 2023-10-18 13:08:42
	- Source Code generated by: Francesco

********* Pleasedontcode.com **********/
/****** DEFINITION OF LIBRARIES *****/
#include <Arduino.h>

/****** SYSTEM REQUIREMENT 1 *****/
/* If the sensor identifies an object closer than 10cm, */
/* activate the relay; if the sensor is at least 20cm away, */
/* deactivate the relay. Use a hysteresis of 10 and 20 cm. */

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);
void updateInputs(void);
void convertInputsFromRawToPhyData(void);

/***** DEFINITION OF ANALOG INPUT PINS *****/
const uint8_t obstacleSensor_PIN_A0 = A0;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t relay_PIN_D2 = 2;

/****** DEFINITION OF ANALOG INPUTS CHARACTERISTIC CURVES *****/
const uint8_t SEGMENT_POINTS_voltage_distance_PIN_A0 = 4;
const float voltage_distance_PIN_A0_lookup[2][SEGMENT_POINTS_voltage_distance_PIN_A0] PROGMEM =
{
    {4.5, 3.8, 2.4, 1.0},   // Voltage [V]
    {10.0, 30.0, 50.0, 200.0}   // distance [cm]
};

/***** DEFINITION OF INPUT RAW VARIABLES *****/
/***** used to store raw data *****/
unsigned int obstacleSensor_PIN_A0_rawData = 0; // Analog Input

/***** DEFINITION OF INPUT PHYSICAL VARIABLES *****/
/***** used to store data after characteristic curve transformation *****/
float obstacleSensor_PIN_A0_phyData = 0.0; // distance [cm]

void setup(void)
{
    // put your setup code here, to run once:
    pinMode(obstacleSensor_PIN_A0, INPUT);
    pinMode(relay_PIN_D2, OUTPUT);
}

void loop(void)
{
    // put your main code here, to run repeatedly:
    updateInputs(); // Refresh input data
    convertInputsFromRawToPhyData(); // Transform raw data to physical data

    // Check if the obstacle is closer than 10cm
    if (obstacleSensor_PIN_A0_phyData < 10.0)
    {
        digitalWrite(relay_PIN_D2, HIGH); // Activate the relay
    }
    // Check if the obstacle is at least 20cm away
    else if (obstacleSensor_PIN_A0_phyData >= 20.0)
    {
        digitalWrite(relay_PIN_D2, LOW); // Deactivate the relay
    }

    delay(100); // Delay for stability
}

void updateInputs()
{
    obstacleSensor_PIN_A0_rawData = analogRead(obstacleSensor_PIN_A0);
}

float lookup_phyData_from_voltage(float voltage, int segment_points, const float* voltage_phyData_lookup)
{
    // Search table for appropriate value.
    uint8_t index = 0;

    const float *voltagePointer = &voltage_phyData_lookup[0];
    const float *phyDataPointer = &voltage_phyData_lookup[segment_points];

    // Perform minimum and maximum voltage saturation based on characteristic curve
    voltage = min(voltage, voltagePointer[segment_points - 1]);
    voltage = max(voltage, voltagePointer[0]);

    while (pgm_read_float(voltagePointer[index]) <= voltage && index < segment_points)
        index++;

    // If index is zero, physical value is smaller than our table range
    if (index == 0)
    {
        return map_f(voltage,
                     pgm_read_float(voltagePointer[0]),   // X1
                     pgm_read_float(voltagePointer[1]),   // X2
                     pgm_read_float(phyDataPointer[0]),   // Y1
                     pgm_read_float(phyDataPointer[1])); // Y2
    }
    // If index is maxed out, physical value is larger than our range.
    else if (index == segment_points)
    {
        return map_f(voltage,
                     pgm_read_float(voltagePointer[segment_points - 2]),   // X1
                     pgm_read_float(voltagePointer[segment_points - 1]),   // X2
                     pgm_read_float(phyDataPointer[segment_points - 2]),   // Y1
                     pgm_read_float(phyDataPointer[segment_points - 1])); // Y2
    }
    // index is between 0 and max, just right
    else
    {
        return map_f(voltage,
                     pgm_read_float(voltagePointer[index - 1]), // X1
                     pgm_read_float(voltagePointer[index]),    // X2
                     pgm_read_float(phyDataPointer[index - 1]), // Y1
                     pgm_read_float(phyDataPointer[index]));  // Y2
    }
}

float map_f(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;
}

void convertInputsFromRawToPhyData()
{
    float voltage = 0.0;

    voltage = obstacleSensor_PIN_A0_rawData * (3.3 / 1023.0);
    obstacleSensor_PIN_A0_phyData = lookup_phyData_from_voltage(voltage, SEGMENT_POINTS_voltage_distance_PIN_A0, &(voltage_distance_PIN_A0_lookup[0][0]));
}