#include <SPI.h>
#include <EEPROM.h>
// I2C
#include <Wire.h>
#include "DFRobot_LCD.h"


unsigned int randNumber;

#define Batt A3

#define Sens1 A2
#define Sens2 A1
#define Sens3 A0

// Threshold 
// Limita, la toate 3 sensoare
#define HitThsMin 50
#define HitThsMax 100

// Deprecated 
#define LED_R 3
#define LED_G 4
#define LED_B 2

// Button for formatting 
#define format 4

DFRobot_LCD lcd(16, 2); //16 characters and 2 lines of show

int hits = 0;
int avgHits = 0;
unsigned long mill = 0;
unsigned long mill1 = 0;
unsigned long mill2 = 0;
// Battery percentage
unsigned long battt = 100;



///RF side

#include <nRF24L01.h>
#include <RF24.h>
RF24 radio; // CE, CSN
// pipe 
const byte address[6] = "00001"; 


struct dataStruct{
  int id;
  char d[30] ;
}
M_d;



void setup() {
  Serial.begin(115200);
  // initialize
  lcd.init();
  lcd.setCursor(1, 0);
  lcd.print("PowerPunchPlus");
  ///////////////////////
  pinMode(format, INPUT_PULLUP);
  delay(3000);

  lcdInit();


  radio.begin(9,10);
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MAX);
  radio.stopListening();

  ///////////////////////
 M_d.id = readIntFromEEPROM(10);

  Serial.print("StartSd\n");
   M_d.d[0]='\0';
  sprintf(M_d.d, "StartSd");
  radio.write(&M_d, sizeof(M_d));
  delay(100);
  Serial.print("StopSd\n");
  M_d.d[0]='\0';
  sprintf(M_d.d, "StopSd");
  radio.write(&M_d, sizeof(M_d));
 
}


void loop() {

  int data1 = analogRead(Sens1);
  int data2 = analogRead(Sens2);
  int data3 = analogRead(Sens3);




  static int MaxData1 = 0;
  static int MaxData2 = 0;
  static int MaxData3 = 0;

  static bool dect1 = false;
  static bool dect2 = false;
  static bool dect3 = false;

  //////////////////////////////////////////
  if (data1 > HitThsMax ) {
    dect1 = true;
    if (data1 > MaxData1) {
      MaxData1 = data1;
    }
  }
  if (data1 < HitThsMin && dect1 ) {
    dect1 = false;
    hits ++;

    LCDOI(MaxData1, 2, 0, 4);
    LCDOI(hits, 2, 1, 4);
    if (hits > 5) {
      LCDOI(AVGD (MaxData1), 11, 0, 4);
    }
    else {
      AVGD (MaxData1);
    }
    sendRF(MaxData1,'r',false);
    Serial.print(MaxData1);
    Serial.print(",r\n");
    MaxData1 = 0;
  }
  /////////////////////////////////////

  if (data2 > HitThsMax ) {
    dect2 = true;
    if (data2 > MaxData2) {
      MaxData2 = data2;
    }
  }
  if (data2 < HitThsMin && dect2 ) {
    dect2 = false;
    hits ++;


    LCDOI(MaxData2, 2, 0, 4);
    LCDOI(hits, 2, 1, 4);
    if (hits > 5) {
      LCDOI(AVGD (MaxData2), 11, 0, 4);
    }
    else {
      AVGD (MaxData2);
    }
    sendRF(MaxData2,'l',false);
    Serial.print(MaxData2);
    Serial.print(",l\n");
    MaxData2 = 0;
  }

  //////////////////////////////////////////////////
  if (data3 > HitThsMax ) {
    dect3 = true;
    if (data3 > MaxData3) {
      MaxData3 = data3;
    }
  }
  if (data3 < HitThsMin && dect3 ) {
    dect3 = false;
    hits ++;

    LCDOI(MaxData3, 2, 0, 4);
    LCDOI(hits, 2, 1, 4);
    if (hits > 5) {
      LCDOI(AVGD (MaxData3), 11, 0, 4);
    }
    else {
      AVGD (MaxData3);
    }
    sendRF(MaxData3,'u',false);
    Serial.print(MaxData3);
    Serial.print(",u\n");
    MaxData3 = 0;
  }

  // la fiecare 10 sec el verifica bateria 
  if (millis() - mill > 10000) {
    int battD = analogRead(Batt);
    battD = map(battD, 670, 860, 0, 100);
    if (battD > 100) {
      battD = 100;
    }
    else if (battD < 0) {
      battD = 0;
    }
    else{
      battt = battD;
      LCDOI(battt, 13, 1, 3);
    }

    mill = millis();
  }


  if (!digitalRead(format)) {
    if (millis() - mill1 > 1000 && millis() - mill1 < 1200) {
      new_set();
    }
   if (millis() - mill1 > 10000 && millis() - mill1 < 10200) {
       pair();
    }
  }
  else {
    mill1 = millis();
  }

}


void LCDOI (int a, int p1, int p2, int s) {
  int sz = strlen(a);
  lcd.setCursor(p1, p2);
  int b = a;
  int count;
  for (int i = 0 ; i < s; i++) {
    lcd.print(" ");
  }
  lcd.setCursor(p1, p2);
  lcd.print(b);
}


void LCDOC (char* a, int p1, int p2, int s) {
  lcd.setCursor(p1, p2);
  for (int i = 0 ; i < s; i++) {
    lcd.print(" ");
  }
  lcd.setCursor(p1, p2);
  lcd.print(a);
}



int AVGD (int a) {
  static int AVG [5];
  for (int i = 4; i > 0; i--) {
    AVG[i] = AVG[i - 1];
  }
  AVG[0] = a;
  int sum = 0;
  for (int i = 0; i < 5; i++) {
    sum += AVG[i];
    //Serial.println(AVG[i]);
  }
  //Serial.println("");
  sum = sum / 5;
  return sum;
}



void new_set() {
  lcd.clear();
  lcd.setCursor(0, 0);
  hits = 0;
  lcd.print("Creating new set...");
  delay(3000);
  M_d.d[0]='\0';
  sprintf(M_d.d, "*****");
  radio.write(&M_d, sizeof(M_d));
  Serial.print("*****\n");
  lcdInit();
}

void lcdInit() {
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("L:");

  lcd.setCursor(7, 0);
  lcd.print("AVG:");

  lcd.setCursor(0, 1);
  lcd.print("H:");


  lcd.setCursor(11, 1);
  lcd.print("B:");
}



void sendRF(int a,char c, bool n){
  
  M_d.d[0]='\0';
  sprintf(M_d.d, "%d,%c", a,c);
  radio.write(&M_d, sizeof(M_d));
}




void pair(){

  lcd.clear();
  lcd.setCursor(0, 0);
  hits = 0;
  lcd.print("Pairing device...");
  delay(2000);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Press the dongle");
  lcd.setCursor(0, 1);
  lcd.print("button for 5 s"); 
  delay(20000);
  randomSeed(analogRead(A3)+analogRead(A2)+analogRead(A1)+analogRead(A0));
  writeIntIntoEEPROM(10, random(100) );
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("generated ID");
  lcd.setCursor(0, 1);
  lcd.print(readIntFromEEPROM(10)); 
  delay(2000);
  lcd.clear();
  Serial.println(readIntFromEEPROM(10));
  sprintf(M_d.d, "pairing");
  M_d.id=readIntFromEEPROM(10);
  while (!radio.write(&M_d, sizeof(M_d))){
  delay(1000);
  }
  lcd.setCursor(0, 0);
  lcd.print("pairing finished");
  delay(2000);
  lcd.clear();
  lcdInit();
}


void writeIntIntoEEPROM(int address, uint8_t number)
{ 
  EEPROM.write(address, number);
}

int readIntFromEEPROM(int address)
{
  return EEPROM.read(address);
}