#include <Wire.h>
#include "MAX30105.h"
#include "heartRate.h"
#include <Servo.h>
MAX30105 particleSensor;
Servo myServo;
// Heart rate variables
const byte RATE_SIZE = 4;
byte rates[RATE_SIZE];
byte rateSpot = 0;
long lastBeat = 0;
float beatsPerMinute;
int beatAvg;
// Servo
const int servoPin = 9;
int targetAngle = 10;   // where we want to go
int currentAngle = 10;  // current position
void setup()
{
 Serial.begin(9600);
 myServo.attach(servoPin);
 Serial.println("Initializing...");
 if (!particleSensor.begin(Wire, I2C_SPEED_STANDARD))
 {
   Serial.println("Sensor not found!");
   while (1);
 }
 particleSensor.setup();
 particleSensor.setPulseAmplitudeRed(0x0A);
 particleSensor.setPulseAmplitudeGreen(0);
 // Initialize BPM values
 for (byte i = 0; i < RATE_SIZE; i++)
 {
   rates[i] = 75;
 }
 Serial.println("Place your finger on the sensor");
}
void loop()
{
 long irValue = particleSensor.getIR();
 // Detect heartbeat
 if (checkForBeat(irValue))
 {
   long delta = millis() - lastBeat;
   lastBeat = millis();
   beatsPerMinute = 60 / (delta / 1000.0);
   if (beatsPerMinute < 255 && beatsPerMinute > 20)
   {
     rates[rateSpot++] = (byte)beatsPerMinute;
     rateSpot %= RATE_SIZE;
     beatAvg = 0;
     for (byte x = 0; x < RATE_SIZE; x++)
       beatAvg += rates[x];
     beatAvg /= RATE_SIZE;
   }
 }
 // Use average BPM
 float bpmToUse = beatAvg;
 int stepSize = 0;
 int minStepSize = 0;
 int maxStepSize = 10;
 if (bpmToUse < 20)
 {
   stepSize = minStepSize;
 }
 else if (bpmToUse > 60)
 {
   stepSize = maxStepSize;
 }
 else
 {
   stepSize = map(bpmToUse, 20, 60, minStepSize, maxStepSize); // speed depends on BPM
 }
 currentAngle = (currentAngle + stepSize) % 179;
 myServo.write(currentAngle);
 // Debug output
 Serial.print("BPM=");
 Serial.print(beatsPerMinute);
 Serial.print(", Avg=");
 Serial.print(beatAvg);
 // Serial.print(", Target=");
 // Serial.print(targetAngle);
 Serial.print(", Current=");
 Serial.print(currentAngle);
 Serial.print(", Step Size=");
 Serial.println(stepSize);
 // No finger → reset
 if (irValue < 50000)
 {
   currentAngle = 10;
   myServo.write(currentAngle);
   Serial.println("No finger detected");
 }
 delay(20); // smooth update
}