#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;
//
const int samplesNumberTake = 200; //Ile próbek
const long interval = 100; //Co ile próbki w ns//zmiana o 1000
//

struct AnalogReadHistory{
  int Analog0;
  int Analog1;
  int Analog2;
  int Analog3;
  int Analog4;
  int Analog5;
  unsigned long readTime;
};

AnalogReadHistory analogReadHistory[200]; //Z zapasek.

void setup() {
 lcd.begin(16, 2);
 lcd.setCursor(0, 0); //Ustawienie kursora
 //lcd.print("Tutaj pojawi sie"); //Wyświetlenie tekstu
 //lcd.setCursor(0, 1); //Ustawienie kursora
 //lcd.print("pomiar napiecia "); //Wyświetlenie tekstu
 Serial.begin(230400);
 Timer1.initialize(interval);
 Timer1.attachInterrupt(takeReading);
 readyForReading = true;

}


void takeReading() {
     analogReadHistory[sampleNumber].Analog1 = analogRead(1);
      analogReadHistory[sampleNumber].Analog2 = analogRead(2);
      analogReadHistory[sampleNumber].Analog3 = analogRead(3);
      //analogReadHistory[sampleNumber].Analog4 = analogRead(4);
      //analogReadHistory[sampleNumber].Analog5 = analogRead(5);
      analogReadHistory[sampleNumber].readTime = millis();
      ++sampleNumber;
}

int amount(int a)
{
  long int suma = 0;
  for(int i=0; i++; i<=samplesNumberTake)
  {
    suma += a;

  }

  return suma/samplesNumberTake;
}

int current (int a)
{
  long int suma = 0;
  for(int i=0; i++; i<=samplesNumberTake)
  {
   a = jedna próbka razy jedna próbka
  suma+= a; // on zlicza mi sume kwadratów
  }
  return(sqrt(suma/samplesNumberTake));
}

bool isSampleCorrect(AnalogReadHistory analogReadHistory){
  Serial.print("A1=");
  Serial.print(current(analogReadHistory.Analog1)  - amount(analogReadHistory.Analog3) );
  Serial.print("A2=");
  Serial.print(current(analogReadHistory.Analog2) - amount(analogReadHistory.Analog3) );
  Serial.print("offset");
  Serial.println(amount(analogReadHistory.Analog3));
  int w1 = (analogReadHistory.Analog1 - 511) - analogReadHistory.Analog3;
  int w2 = (analogReadHistory.Analog2 -511) - analogReadHistory.Analog3;
  if (abs(w1-w2) <= 40) 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);
        for (int i = 0; i <= samplesNumberTake; ++i){
          if (isSampleCorrect(analogReadHistory[i])) ++correctSamples;
        }
        if (correctSamples < 17){
          lcd.print("  Rozne prady   ");
          Serial.print("Niepoprawne odczyty: ");
          Serial.println("BAD");
        }
        else
        {
           lcd.setCursor(0, 0);
           lcd.print("Dziala poprawnie");
           Serial.println("GOOD");
           Serial.print("Poprawne odczyty: ");
          }
        //Serial.println(sampleNumber-correctSamples);
        sampleNumber = 0;
        _delay_ms(2000);
        interrupts(); // Włącza Timer
      }
//     if (currentMillis - previousMillis >= interval) {
//         //Ustawienie kursora
//        lcd.print("Voltage "); //Wyświetlenie tekstu
//        lcd.print(voltage);
//       // lcd.setCursor(0, 1); //Ustawienie kursora
//       // lcd.print("mi nie wychodzi "); //Wyświetlenie tekstu
//        //wywala dane do komputera
//        Serial.print(analogReadHistory.Analog2);
//        Serial.print(" ");
//        Serial.print(analogReadHistory.Analog3);
//        Serial.print(" ");
//        Serial.println(analogReadHistory.Analog4);
//        Serial.println(analogReadHistory.readTime);
//        lastDataProcessed = true;
//    }

}