#include <stdio.h>
#include <Arduino.h>
#include <LiquidCrystal_I2C.h>
#include "lib.hpp"

#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

#define BUFFER_SIZE 10
#define PLOT_RECURRENCE 10
#define NOISE_RANDOM_SCALE 30
#define THERMISTOR_ANALOG_PIN A0
#define PIN_TRIG 2
#define PIN_ECHO 3
#define BAUD_RATE 9600
#define LOOP_DELAY 50
#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 20
#define LCD_ROWS 2
#define CELSIUS_SYMBOL (char)223

int *tempBuffer = new int[BUFFER_SIZE];
int *distanceBuffer = new int[BUFFER_SIZE];
float *weights = new float[BUFFER_SIZE];
int bufferIndex = 0;
int plotRecurrence = PLOT_RECURRENCE;
int tempAverage = 0;
int distanceAverage = 0;
int tempWeightedAverage =0 ;
int distanceWeightedAverage = 0;




void my_putChar(char ch, FILE *f) {
  Serial.write(ch);
}

char my_GetChar(FILE * f){
  while(!Serial.available());
  return Serial.read();
}

void setup() 
{
  Serial.begin(9600);
  lcd.begin(16, 2);
  // 5. IO Stream Definition
  FILE *my_stream = fdevopen(my_putChar,my_GetChar);
  // 6. link just defined strem to STDIO
  // (once per session)
  stdin = stdout = my_stream ;

  pinMode(THERMISTOR_ANALOG_PIN, INPUT);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  for(int i = 0; i < BUFFER_SIZE; i++){
    weights[i] = 1.0/BUFFER_SIZE; 
  }
}

void loop() 
{
  int analogValue = analogRead(THERMISTOR_ANALOG_PIN);
  int rawTemp = addRandom(analogValue, NOISE_RANDOM_SCALE);
  tempBuffer[bufferIndex] = rawTemp;

  long duration, distance;
  digitalWrite(PIN_TRIG, LOW);  
  delayMicroseconds(2); 
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10); 
  digitalWrite(PIN_TRIG, LOW);
  duration = pulseIn(PIN_ECHO, HIGH);
  distance = (duration/2) / 29.1;
  distance = addRandom(distance, NOISE_RANDOM_SCALE);
  distanceBuffer[bufferIndex] = distance;
  bufferIndex++;

  if (bufferIndex >= BUFFER_SIZE) 
  {
    // Then calculate mean and clear buffers
    tempAverage = analogToCelsius(getMeanAndClear(tempBuffer, BUFFER_SIZE));
    distanceAverage = getMeanAndClear(distanceBuffer, BUFFER_SIZE);
    rawTemp=analogToCelsius(rawTemp);

    // Calculate weighted averages first
    tempWeightedAverage = analogToCelsius(getWeightedAverage(tempBuffer, weights, BUFFER_SIZE));
    distanceWeightedAverage = getWeightedAverage(distanceBuffer, weights, BUFFER_SIZE);

    printf("####\n");

    // Temperature data
    printf("Temp Raw: %d\n", rawTemp);
    printf("Temp Avg: %d\n", tempAverage);
    printf("Temp Weighted Avg: %d\n", tempWeightedAverage);
    printf("----------\n");
    // Distance data
    printf("Dist Raw: %d\n", distance);
    printf("Dist Avg: %d\n", distanceAverage);
    printf("Dist Weighted Avg: %d\n", distanceWeightedAverage);
    printf("\n");


    bufferIndex = 0;
    clearBuffer(tempBuffer, BUFFER_SIZE);
    clearBuffer(distanceBuffer, BUFFER_SIZE);

    lcd.clear(); // Clear the LCD before displaying new data
    lcd.setCursor(0, 0); // Set cursor to the first line
    // lcd.print("T:R-"); lcd.print(rawTemp); 
    lcd.print("T:A-"); lcd.print(tempAverage);
    lcd.print(" W-"); lcd.print(tempWeightedAverage);

    lcd.setCursor(0, 1); // Set cursor to the second line
    // lcd.print("D:R-"); lcd.print(distance); 
    lcd.print("D:A-"); lcd.print(distanceAverage);
    lcd.print(" W-"); lcd.print(distanceWeightedAverage);
  }
  delay(LOOP_DELAY);
}