#include <Wire.h> 
#include <math.h> 
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <SPI.h>
#include <MFRC522.h>
#include "MAX30105.h"
#include "heartRate.h"
#include "spo2_algorithm.h"

#define MAX_BRIGHTNESS 255
#define SS_PIN 10
#define RST_PIN 9

MAX30105 particleSensor;
TinyGPSPlus gps;
MFRC522 mfrc522(SS_PIN, RST_PIN);

#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
byte readCard[4];
String tag_UID = "D1 59 91 2D";  // Replace this with the UID of your tag!!!
String tagID = "";
uint16_t irBuffer[100]; //infrared LED sensor data
uint16_t redBuffer[100];  //red LED sensor data
#else
uint32_t irBuffer[100]; //infrared LED sensor data
uint32_t redBuffer[100];  //red LED sensor data
#endif
int32_t bufferLength; //data length
int32_t spo2; //SPO2 value
int8_t validSPO2; 
int32_t heartRate; //heart rate value
int8_t validHeartRate; //indicator to show if the heart rate calculation is valid
const byte RATE_SIZE = 4; //Increase this for more averaging. 4 is good.
byte rates[RATE_SIZE]; //Array of heart rates
byte rateSpot = 0;
long lastBeat = 0; //Time at which the last beat occurred
float beatsPerMinute,temp,tempF;
const int MPU = 0x68; 
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; 
int AcXcal,AcYcal,AcZcal,GyXcal,GyYcal,GyZcal,tcal,lat,lon,db1,db; 
int dx,dy,dz,dx1,dy1,dz1,dgx,dgy,dgz,dgx1,dgy1,dgz1,x,beatAvg,AA,AB,AC,AD,AE,AF,AG,AH,AI;
bool accident,abnormal;
double t,tx,tf;
byte pulseLED = 11; //Must be on PWM pin
byte readLED = 13; //Blinks with each data read
int AcD = 300; //change in value of AC
int GyD = 300; //change in value of GY
int BD = 20; // change in vaule of BPM
int RXPin = 2;
int TXPin = 3;
String time,tim,cow,s1,read;

SoftwareSerial gpsSerial(RXPin, TXPin);
SoftwareSerial mySerial(3, 2);

void setup()
{
  Serial.println(F("Initializing..."));
  if (!particleSensor.begin(Wire, I2C_SPEED_FAST)) {
    Serial.println(F("MAX30102 was not found. Please check wiring/power. "));
    while (1);
  }
  Serial.println(F("Place your index finger on the sensor with steady pressure."));
  particleSensor.setup(); //Configure sensor with default settings
  particleSensor.setPulseAmplitudeRed(0x0A); //Turn Red LED to low to indicate sensor is running
  particleSensor.setPulseAmplitudeGreen(0); //Turn off Green LED
  pinMode(pulseLED, OUTPUT);
  pinMode(readLED, OUTPUT);
  Wire.begin(); 
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0); 
  Wire.endTransmission(true); 
  Serial.begin(115200); 
  gpsSerial.begin(9600);
  mySerial.begin(9600);
  Serial.println(F("Initializing..."));
  delay(1000);

  mySerial.println(F("AT")); //Once the handshake test is successful, it will back to OK
  updateSerial();
  mySerial.println(F("AT+CSQ")); //Signal quality test, value range is 0-31 , 31 is the best
  updateSerial();
  mySerial.println(F("AT+CCID")); //Read SIM information to confirm whether the SIM is plugged
  updateSerial();
  mySerial.println(F("AT+CREG?")); //Check whether it has registered in the network
  updateSerial();
  mySerial.println(F("AT+CCLK?")); //Check whether it has registered in the network
  updateSerial();
  byte ledBrightness = 60; //Options: 0=Off to 255=50mA
  byte sampleAverage = 4; //Options: 1, 2, 4, 8, 16, 32
  byte ledMode = 2; //Options: 1 = Red only, 2 = Red + IR, 3 = Red + IR + Green
  byte sampleRate = 100; //Options: 50, 100, 200, 400, 800, 1000, 1600, 3200
  int pulseWidth = 411; //Options: 69, 118, 215, 411
  int adcRange = 4096; //Options: 2048, 4096, 8192, 16384

  particleSensor.setup(ledBrightness, sampleAverage, ledMode, sampleRate, pulseWidth, adcRange); //Configure sensor with these settings
}

void loop()
{
 bufferLength = 100; //buffer length of 100 stores 4 seconds of samples running at 25sps
  updateSerial();
  //read the first 100 samples, and determine the signal range
  for (byte i = 0 ; i < bufferLength ; i++)
  {
    while (particleSensor.available() == false) //do we have new data?
    particleSensor.check(); //Check the sensor for new data

    redBuffer[i] = particleSensor.getRed();
    irBuffer[i] = particleSensor.getIR();
    particleSensor.nextSample(); //We're finished with this sample so move to next sample

  }

  while (gpsSerial.available() > 0)
    if (gps.encode(gpsSerial.read()))
      displayInfo();

  // If 5000 milliseconds pass and there are no characters coming in
  // over the software serial port, show a "No GPS detected" error
  if (millis() > 5000 && gps.charsProcessed() < 10)
  {
    Serial.println(F("No GPS detected"));
    while(true);
  }

  while (readID())
  {
    if (tagID == tag_UID)
    {
      cow = "OD_Cattle_1958R";
      mySerial.println(F("AT+CMGF=1")); // Configuring TEXT mode
      updateSerial();
      mySerial.println(F("AT+CMGS=\"+91xxxxxxxxxx\""));//change ZZ with country code and xxxxxxxxxxx with phone number to sms
      updateSerial();
      s1 = "\n";
      mySerial.println(s1 + "ID : " + cow + "\nWeight: 439 Kg \nHeight: 124 cm");
      updateSerial();
      mySerial.write(26);
      digitalWrite(A2,1);
    }
  }

  //calculate heart rate and SpO2 after first 100 samples (first 4 seconds of samples)
  maxim_heart_rate_and_oxygen_saturation(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &heartRate, &validHeartRate);

  //Continuously taking samples from MAX30102.  Heart rate and SpO2 are calculated every 1 second
  while (1)
  {
    //dumping the first 25 sets of samples in the memory and shift the last 75 sets of samples to the top
    for (byte i = 25; i < 100; i++)
    {
      redBuffer[i - 25] = redBuffer[i];
      irBuffer[i - 25] = irBuffer[i];
    }

    //take 25 sets of samples before calculating the heart rate.
    for (byte i = 75; i < 100; i++)
    {
      while (particleSensor.available() == false) //do we have new data?
        particleSensor.check(); //Check the sensor for new data

      digitalWrite(readLED, !digitalRead(readLED)); //Blink onboard LED with every data read

      redBuffer[i] = particleSensor.getRed();
      irBuffer[i] = particleSensor.getIR();
      particleSensor.nextSample(); //We're finished with this sample so move to next sample

      Serial.print(F(", SPO2="));
      Serial.print(spo2, DEC);
    }

    //After gathering 25 new samples recalculate HR and SP02
    maxim_heart_rate_and_oxygen_saturation(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &heartRate, &validHeartRate);
	
    Wire.beginTransmission(MPU); 
    Wire.write(0x3B); 
    Wire.endTransmission(false); 
    Wire.requestFrom(MPU,14,true); 
    long irValue = particleSensor.getIR();
    
    db1 = beatsPerMinute;

  if (checkForBeat(irValue) == true) {
    //We sensed a beat!
    long delta = millis() - lastBeat;
    lastBeat = millis();

    beatsPerMinute = 60 / (delta / 1000.0);

    if (beatsPerMinute < 255 && beatsPerMinute > 20) {
      rates[rateSpot++] = (byte)beatsPerMinute; //Store this reading in the array
      rateSpot %= RATE_SIZE; //Wrap variable

      //Take average of readings
      beatAvg = 0;
      for (byte x = 0 ; x < RATE_SIZE ; x++)
        beatAvg += rates[x];
      beatAvg /= RATE_SIZE;
    }
  }
  
  
  Serial.print(F(", BPM="));
  Serial.print(beatsPerMinute);
  Serial.print(F(", Avg BPM="));
  Serial.print(beatAvg);

  if (irValue < 50000)
  {
    Serial.print(F(" No finger?"));
  }
  
  Serial.println();

    //Acceleration data correction
    AcXcal = 0;
    AcYcal = 0;
    AcZcal = 0;

    //Temperature correction
    tcal = -1600;

    //Gyro correction
    GyXcal = 0;
    GyYcal = 0;
    GyZcal = 0;

    dx1 = AcX + AcXcal;
    dy1 = AcY + AcYcal;
    dz1 = AcZ + AcZcal;

    dgx1 = GyX + GyXcal;
    dgy1 = GyY + GyYcal;
    dgz1 = GyZ + GyZcal;

    //read accelerometer data
    AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) 0x3C (ACCEL_XOUT_L)  
    AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) 0x3E (ACCEL_YOUT_L) 
    AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) 0x40 (ACCEL_ZOUT_L)
  
    //read temperature data 
    Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) 0x42 (TEMP_OUT_L) 
  
    //read gyroscope data
    GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) 0x44 (GYRO_XOUT_L)
    GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) 0x46 (GYRO_YOUT_L)
    GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) 0x48 (GYRO_ZOUT_L) 

    //temperature calculation
    tx = Tmp + tcal;
    t = tx/340 + 36.53; //equation for temperature in degrees C from datasheet
    tf = (t * 9/5) + 32; //fahrenheit

    tempF = particleSensor.readTemperatureF();
    temp = particleSensor.readTemperature();

    if (x > 10)
    {
      dx = AcX + AcXcal;
      dy = AcY + AcYcal;
      dz = AcZ + AcZcal;

      dgx = GyX + GyXcal;
      dgy = GyY + GyYcal;
      dgz = GyZ + GyZcal;
      
      db = beatsPerMinute;

      AA = dx - dx1;
      AB = dy - dy1;
      AC = dz - dz1;
      AD = dgx - dgx1;
      AE = dgy - dgy1;
      AF = dgz - dgz1;
      AG = db - db1;
      if (((abs(AA))>AcD) || ((abs(AB))>AcD) || ((abs(AC))>AcD) || ((abs(AD))>GyD) || ((abs(AE))>GyD) || ((abs(AF))>GyD) && ((abs(AG)>BD)))
      {
        accident = true;
      }
      else if (((abs(AG)>BD)) || spo2>100 && tempF>=103 || temp>39.5)
      {
        abnormal = true;
      }
    }

    if (accident==true)
    {
      mySerial.println(F("AT+CCLK?"));
      while(mySerial.available()) 
      {
        Serial.write(mySerial.read());//Forward what Software Serial received to Serial Port
        time = mySerial.read();
        tim = time.substring(7,25);
      }
      mySerial.println(F("AT+CMGF=1")); // Configuring TEXT mode
      updateSerial();
      mySerial.println(F("AT+CMGS=\"+91xxxxxxxxxx\""));//change ZZ with country code and xxxxxxxxxxx with phone number to sms
      updateSerial();
      s1 = "\n";
      mySerial.print(s1 + "Collision detected!!!" + "\nCattle no. " + cow + " needs immediate help at: \n Lat:"  + lat + "\n Lon:" + lon + "\nTime :" + tim + "\nVitals: \nBPM: " + beatsPerMinute + "\nSpo2: " + spo2 +"\nTemp: " + tempF + "\ " + temp );
      updateSerial();
      mySerial.write(26);
      mySerial.println(F("ATD+ +91xxxxxxxxxx;")); //  change ZZ with country code and xxxxxxxxxxx with phone number to dial
      updateSerial();
      delay(10000); // wait for 20 seconds...
      mySerial.println(F("ATH")); //hang up
      updateSerial();
      mySerial.println(F("AT+CNMI=1,2,0,0,0"));
      while(mySerial.available()) 
      {
        Serial.write(mySerial.read());//Forward what Software Serial received to Serial Port
        read = mySerial.read();
        if (read == "Stop")
        {
          accident=false;
          mySerial.println(F("AT+CMGS=\"+91xxxxxxxxxx\""));//change ZZ with country code and xxxxxxxxxxx with phone number to sms
          updateSerial();
          mySerial.println(F("Issue resolved"));
          mySerial.write(26);
        }
      }
      digitalWrite(9,1);
    }
    if (abnormal==true)
    {
      mySerial.println(F("AT+CCLK?"));
      while(mySerial.available()) 
      {
        Serial.write(mySerial.read());//Forward what Software Serial received to Serial Port
        time = mySerial.read();
        tim = time.substring(7,25);
      }
      mySerial.println(F("AT+CMGF=1")); // Configuring TEXT mode
      updateSerial();
      mySerial.println(F("AT+CMGS=\"+91xxxxxxxxxx\""));//change ZZ with country code and xxxxxxxxxxx with phone number to sms
      updateSerial();
      s1 = "\n";
      mySerial.print(s1 + "Abnormality detected!!!" + "\nCattle no. " + cow + " needs immediate help at: \n Lat:"  + lat + "\n Lon:" + lon + "\nTime :" + tim + "\nVitals: \nBPM: " + beatsPerMinute + "\nSpo2: " + spo2 +"\nTemp: " + tempF + "\ " + temp );
      updateSerial();
      mySerial.write(26);    
      if (read == "Stop")
        {
          abnormal=false;
          mySerial.println(F("AT+CMGS=\"+91xxxxxxxxxx\""));//change ZZ with country code and xxxxxxxxxxx with phone number to sms
          updateSerial();
          mySerial.println(F("Cattle treated"));
        }  
      digitalWrite(10,1);
    }

    Serial.print(F("Accelerometer: "));
    Serial.print(F("X = ")); Serial.print(dx1);
    Serial.print(F(" Y = ")); Serial.print(dy1);
    Serial.print(F(" Z = ")); Serial.println(dz1); 

    Serial.print(F("Temperature in celsius = ")); Serial.print(t);  
    Serial.print(F(" fahrenheit = ")); Serial.println(tf);  
  
    Serial.print(F("Gyroscope: "));
    Serial.print(F("X = ")); Serial.print(dgx1);
    Serial.print(F(" Y = ")); Serial.print(dgy1);
    Serial.print(F(" Z = ")); Serial.println(dgz1);
  
    Serial.print(F("\n"));
    delay(1500);

    
    x += 1;
}
}
void displayInfo()
{
  if (gps.location.isValid())
  {
    Serial.print(F("Latitude: "));
    Serial.println(gps.location.lat(), 6);
    Serial.print(F("Longitude: "));
    Serial.println(gps.location.lng(), 6);
    Serial.print(F("Altitude: "));
    Serial.println(gps.altitude.meters());
    lat=gps.location.lat();
    lon=gps.location.lng();
  }
  else
  {
    Serial.println(F("Location: Not Available"));
  }
  
  Serial.println();
  Serial.println();
  delay(1000);
}


void updateSerial()
{
  delay(500);
  while (Serial.available()) 
  {
    mySerial.write(Serial.read());//Forward what Serial received to Software Serial Port
  }
  while(mySerial.available()) 
  {
    Serial.write(mySerial.read());//Forward what Software Serial received to Serial Port
  }
}
boolean readID()
  {
    //Check if a new tag is detected or not. If not return.
    if ( ! mfrc522.PICC_IsNewCardPresent())
    {
      return false;
    }
    //Check if a new tag is readable or not. If not return.
    if ( ! mfrc522.PICC_ReadCardSerial())
    {
      return false;
    }
    tagID = "";
    // Read the 4 byte UID
    for ( uint8_t i = 0; i < 4; i++)
    {
      //readCard[i] = mfrc522.uid.uidByte[i];
      tagID.concat(String(mfrc522.uid.uidByte[i], HEX)); // Convert the UID to a single String
    }
    tagID.toUpperCase();
    mfrc522.PICC_HaltA(); // Stop reading
    return true;
  }