#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "output.h"

// Pins
#define TRIG_PIN 9
#define TEMP_PIN A0
#define JOYSTICK_X A1
#define JOYSTICK_Y A2
#define BUFF_SIZE 5
#define BETA 3950.0

char output_str[10];
int temp_pos = 0, temp_analog_value;
int joystick_pos = 0, joystick_x_analog_value, joystick_y_analog_value;
float celsius, salt_pepper_result, weighted_average_result, weighted_num, weighted_den;
float temp_arr[BUFF_SIZE], temp_arr_copy[BUFF_SIZE], weights[BUFF_SIZE];

void setup() {
  stdio_init_all();
  set_weights();
  printf("%s\n", "Temperature & Joystick");
  printf("%s", "Weights: ");
  for (int i = 0; i < BUFF_SIZE; i++) {
    dtostrf(weights[i], 2, 2, output_str);
    printf("%s, ", output_str);
  }
}

void loop() {
  display_temperature();
  display_joystick();
  delay(1000);
}

void display_temperature() {
  temp_analog_value = analogRead(TEMP_PIN);
  printf("%s", "Analog temperature: ");
  dtostrf(temp_analog_value, 4, 2, output_str);
  printf("%s\n", output_str);

  // Analog to °C
  celsius = 1 / (log(1 / (1023. / temp_analog_value - 1)) / BETA + 1.0 / 298.15) - 273.15;
  printf("%s", "Physical temperature: ");
  dtostrf(celsius, 4, 2, output_str);
  printf("%s °C\n", output_str);

  // Salt and pepper
  salt_pepper_result = salt_pepper_filter_temp();
  printf("%s", "\nSalt and pepper temperature: ");
  dtostrf(salt_pepper_result, 4, 2, output_str);
  printf("%s °C\n", output_str);

  // Weighted average filter
  weighted_average_result = weighted_average_filter(temp_arr);
  printf("%s", "Weighted average filter temperature: ");
  dtostrf(weighted_average_result, 4, 2, output_str);
  printf("%s °C\n", output_str);
}

void display_joystick() {
  joystick_x_analog_value = analogRead(JOYSTICK_X);
  joystick_y_analog_value = analogRead(JOYSTICK_Y);
  
  printf("Joystick X: %d, Joystick Y: %d\n", joystick_x_analog_value, joystick_y_analog_value);
}

float salt_pepper_filter_temp() {
  // Reset the index
  if (temp_pos > BUFF_SIZE) {
    temp_pos = 0;
  }
  temp_arr[temp_pos] = celsius;
  temp_pos++;

  // Copy the original array
  memcpy(temp_arr_copy, temp_arr, sizeof(temp_arr));
  // Sort the copied array
  qsort(temp_arr_copy, BUFF_SIZE, sizeof(float), compare_float);

  printf("%s", "Sorted temperatures: ");
  for (int i = 0; i < BUFF_SIZE; i++) {
    dtostrf(temp_arr_copy[i], 4, 2, output_str);
    printf("%s°C, ", output_str);
  }

  return temp_arr_copy[BUFF_SIZE - 3];
}

float weighted_average_filter(float arr[]) {
  weighted_num = 0;

  for (int i = 0; i < BUFF_SIZE; i++) {
    weighted_num += arr[i] * weights[i];
  }

  return weighted_num / weighted_den;
}

void set_weights() {
  for (int i = 0; i < BUFF_SIZE; i++) {
    weights[i] = 1.0 / BUFF_SIZE;
    weighted_den += weights[i];
  }
}

int compare_float(const void *a, const void *b) {
  float float_a = *((float*)a);
  float float_b = *((float*)b);

  if (float_a < float_b) return -1;
  if (float_a > float_b) return 1;
  return 0;
}