#include <LiquidCrystal.h> //Dołączenie bilbioteki
#include <TimerOne.h>
LiquidCrystal lcd(2, 3, 4, 5, 6, 7); //Informacja o podłączeniu nowego wyświetlacza


using namespace std;

bool readyForReading = false;
int sampleNumber = 0;
//int suma = 0;
//
const int samplesNumberTake = 80; //Ile próbek
const long interval = 250 ; //czas w ns
//

struct AnalogReadHistory{
  int Analog0;
  int Analog1;
  int Analog2;
  int Analog3;
};

AnalogReadHistory analogReadHistory[100]; //Z zapasek.

void setup() {
 lcd.begin(16, 2);
 lcd.setCursor(0, 0); //Ustawienie kursora
 pinMode(8, OUTPUT);
 pinMode(9, OUTPUT);
 Serial.begin(230400);
 Timer1.initialize(interval);
 Timer1.attachInterrupt(takeReading);
 readyForReading = true;
 }


void takeReading() {
  if (sampleNumber < samplesNumberTake){

     analogReadHistory[sampleNumber].Analog1 = analogRead(1);
     analogReadHistory[sampleNumber].Analog2 = analogRead(2);
     analogReadHistory[sampleNumber].Analog3 = analogRead(3);
     ++sampleNumber;
  }
}

double currentForA1(AnalogReadHistory *analogReadHistory)
{
  double suma = 0;
  for(int i=0; i<= samplesNumberTake; i++)
  {
    suma += pow (analogReadHistory[i].Analog1, 2);
  }
  return(sqrt(suma/samplesNumberTake));
}

double currentForA2(AnalogReadHistory *analogReadHistory)
{
  double suma = 0;
  for(int i=0; i<= samplesNumberTake; i++)
  {
     suma += pow (analogReadHistory[i].Analog2, 2);
  }
  return(sqrt(suma/samplesNumberTake));
}

double amount(AnalogReadHistory *analogReadHistory)
{
  double suma = 0;
  for(int i=0; i<= samplesNumberTake; i++)
  {
    suma += analogReadHistory[i].Analog3;
  }

  return suma/(samplesNumberTake-1);
}


bool isSampleCorrect(AnalogReadHistory *analogReadHistory){

  double w1 = currentForA1(analogReadHistory)  - amount(analogReadHistory);
  double w2 = currentForA2(analogReadHistory)  - amount(analogReadHistory);
  Serial.println("  ");
  Serial.print("wejście=");
  Serial.print(w1 );
  Serial.print("  wyjście=");
  Serial.print(w2);
  Serial.print("  offset=");
  Serial.println(amount(analogReadHistory));

  if (abs(w1-w2) <= 35.0) return true;
  else return false;

}


void loop() {
      if (sampleNumber >= samplesNumberTake){
        unsigned long currentMillis = millis();
        int correctSamples = 0;
        noInterrupts(); // Wyłącza Timer
        lcd.setCursor(0, 0);

        if (isSampleCorrect(analogReadHistory) == false){
          lcd.setCursor(0, 0);
          lcd.print("  Rozne prady   ");
          Serial.println("Urzadzenie nie dziala poprawnie: ");
          Serial.println("BAD");
          digitalWrite(8, HIGH);
          digitalWrite(9, LOW);
        }
        else
        {
           lcd.setCursor(0, 0);
           lcd.print("Dziala poprawnie");
           Serial.println("GOOD");
           Serial.println("Urzadzenie dziala poprawnie: ");
           digitalWrite(8, LOW);
           digitalWrite(9, HIGH);

          }
         sampleNumber = 0;
        _delay_ms(2000);
        interrupts(); // Włącza Timer
      }
}