// Distance sensor: https://docs.wokwi.com/parts/wokwi-hc-sr04
// HC-SR04 datasheet: https://cdn.sparkfun.com/datasheets/Sensors/Proximity/HCSR04.pdf
// pulseIn function: https://www.arduino.cc/reference/en/language/functions/advanced-io/pulsein/

// Temperature sensor (with conversion formulas): https://docs.wokwi.com/parts/wokwi-ntc-temperature-sensor
// Analog to voltage: https://docs.arduino.cc/built-in-examples/basics/ReadAnalogVoltage/
// analogRead: https://www.arduino.cc/reference/en/language/functions/analog-io/analogread/

// Include necessary libraries
#include <stdio.h>

// Define global file object for output redirection
FILE output_file;

// Custom function to write characters to Serial
int custom_putchar(char character, __attribute__((unused)) FILE *file)
{
    return !Serial.write(character);
}

// Function to redirect output to Serial
void redirectSerialOutput()
{
    // Setup output stream to use custom putchar function
    fdev_setup_stream(&output_file, custom_putchar, nullptr, _FDEV_SETUP_WRITE);
    // Redirect standard output to the output file stream
    stdout = &output_file;
}

// Pin of distance echo receiver
#define DISTANCE_SENSOR_ECHO_PIN 2
// Pin of distance trigger pulser
#define DISTANCE_SENSOR_TRIG_PIN 3

// Pin of temperature sensor
#define TEMPERATURE_SENSOR_PIN A2
// Resistor value of the included resistor
#define TEMPERATURE_SENSOR_RESISTOR 10000.0

// Baud rate of the serial communication
#define SERIAL_BAUD 9600

// Delay between the loop iterations in ms
#define LOOP_DELAY 500

// Number of readings to be kept in the history
#define READINGS_COUNT 4
// Weights for the weighted average filter in %
#define WEIGHTS (int[]){50, 25, 15, 10}

// Arrays to keep the readings history
int distanceReadings[READINGS_COUNT];     // cm [2, 400]
int temperatureReadings[READINGS_COUNT];  // pin in [0; 1023]

// Read the distance data from the sensor
int readDistance(){
    // Send a 10us pulse with the trigger pin
    digitalWrite(DISTANCE_SENSOR_TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(DISTANCE_SENSOR_TRIG_PIN, LOW);

    // Measure the duration of the echo pulse in uS
    int duration = pulseIn(DISTANCE_SENSOR_ECHO_PIN, HIGH);

    // Minimum measurment cycle in ms
    delay(60);

    // Reuturn the distance in cm
    return (int)(duration / 58);
}

// Push the distance reading to the readings array
void measureDistance(){
    // Move the readings one step back
    for(int i = 0; i < READINGS_COUNT - 1; i++){
        distanceReadings[i] = readDistance();
    }

    // Read the new distance
    distanceReadings[READINGS_COUNT - 1] = readDistance();
}

// Get the salt'n'peper filtered distance
int getDistanceSP(){
    int sortedData[READINGS_COUNT];

    // Copy the readings to a new array
    for(int i = 0; i < READINGS_COUNT; i++){
        sortedData[i] = distanceReadings[i];
    }

    // Sort the copied array with bubble sort
    for(int i = 0; i < READINGS_COUNT; i++){
        for(int j = i + 1; j < READINGS_COUNT; j++){
            if(sortedData[i] > sortedData[j]){
                int temp = sortedData[i];
                sortedData[i] = sortedData[j];
                sortedData[j] = temp;
            }
        }
    }

    // Return the median value
    if(READINGS_COUNT % 2 == 0){
        return sortedData[READINGS_COUNT / 2];
    }
    else{
        return sortedData[(READINGS_COUNT + 1) / 2];
    }
}

// Get the weighted average filtered distance
float getDistanceVA(){
    // Store the weighted average in a float variable
    float distanceVA = 0;

    // Compute the weighted average
    for(int i = 0; i < READINGS_COUNT; i++){
        distanceVA += distanceReadings[i] * WEIGHTS[i] / 100;
    }

    return distanceVA;
}

// Read the temperature data from the sensor
int readTemperatureData(){
    // Read the analog value from the temperature sensor
    int reading = analogRead(TEMPERATURE_SENSOR_PIN);

    // Return the temperature sensor reading
    return reading;
}

// Push the temperature sensor reading to the readings array
void measureTemperature(){
    // Move the readings one step back
    for(int i = 0; i < READINGS_COUNT - 1; i++){
        temperatureReadings[i] = readTemperatureData();
    }

    // Read the new temperature sensor reading
    temperatureReadings[READINGS_COUNT - 1] = readTemperatureData();
}

// Get the salt'n'peper filtered temperature
int getTemperatureSP(){
    int sortedData[READINGS_COUNT];

    // Copy the readings to a new array
    for(int i = 0; i < READINGS_COUNT; i++){
        sortedData[i] = temperatureReadings[i];
    }

    // Sort the copied array with bubble sort
    for(int i = 0; i < READINGS_COUNT; i++){
        for(int j = i + 1; j < READINGS_COUNT; j++){
            if(sortedData[i] > sortedData[j]){
                int temp = sortedData[i];
                sortedData[i] = sortedData[j];
                sortedData[j] = temp;
            }
        }
    }

    // Return the median value
    if(READINGS_COUNT % 2 == 0){
        return sortedData[READINGS_COUNT / 2];
    }
    else{
        return sortedData[(READINGS_COUNT + 1) / 2];
    }
}

// Get the weighted average filtered temperature
float getTemperatureVA(){
    // Store the weighted average in a float variable
    float temperatureVA = 0;

    // Compute the weighted average
    for(int i = 0; i < READINGS_COUNT; i++){
        temperatureVA += temperatureReadings[i] * WEIGHTS[i] / 100;
    }

    return temperatureVA;
}

// Convert the data to voltage
float dataToVoltage(int data){
    return (float)data * 5.0 / 1023.0;
}

// Convert the voltage to resistance
float voltageToResistance(float voltage){
    return (TEMPERATURE_SENSOR_RESISTOR * voltage) / (5.0 - voltage);
}

// Convert the resistance to temperature
float resistanceToTemperature(float resistance){
    return 1.0 / (1.0 / 298.15 + 1.0 / 3950.0 * log(resistance / TEMPERATURE_SENSOR_RESISTOR)) - 273.15;
}

// Convert the data to temperature
float dataToTemperature(int data){
    float voltage = dataToVoltage(data);
    float resistance = voltageToResistance(voltage);

    return resistanceToTemperature(resistance);
}

void setup(){
    // Initialize the serial communication
    Serial.begin(SERIAL_BAUD);
    redirectSerialOutput();

    // Set the trigger pin as output and the echo pin as input
    pinMode(DISTANCE_SENSOR_TRIG_PIN, OUTPUT);
    pinMode(DISTANCE_SENSOR_ECHO_PIN, INPUT);

    // Set the temperature sensor pin as input
    pinMode(TEMPERATURE_SENSOR_PIN, INPUT);

    // Populate the readings array with initial values
    for(int i = 0; i < READINGS_COUNT; i++){
        distanceReadings[i] = readDistance();
        temperatureReadings[i] = readTemperatureData();
    }
}

void loop(){
    // Print an empty line to separate the readings
    printf("\n\r");

    // Measure the distance
    measureDistance();
    // Get s'n'p filtered distance
    int distanceSP = getDistanceSP();
    // Get weighted average filtered distance
    float distanceVA = getDistanceVA();
    // Print the distance readings
    printf("DistanceSP: %d cm\n\r", (int)distanceSP);
    printf("DistanceVA: %d cm\n\r", (int)distanceVA);

    // Measure the temperature
    measureTemperature();
    // Get s'n'p filtered temperature
    int temperatureSP = dataToTemperature(getTemperatureSP());
    // Get weighted average filtered temperature
    float temperatureVA = dataToTemperature(getTemperatureVA());
    // Print the temperature readings
    printf("TemperatureSP: %d C\n\r", (int)temperatureSP);
    printf("TemperatureVA: %d C\n\r", (int)temperatureVA);

    // Delay the loop
    delay(LOOP_DELAY);
}