#include "SevSeg.h"

// Initialize 7-segment display object
SevSeg sevseg; 

// Pins for the 3 digits
byte digitPins[] = {13, 12, 9}; 
// Pins for segments A, B, C, D, E, F, G
byte segmentPins[] = {A2, 11, 4, 3, 10, A1, 5}; 

// Pin for the flex sensor signal
const int flexPin = A0;
// Pin for the pushbutton reset
const int buttonPin = 8;

// Variable to store number of completed reps
int pushupCount = 0;

// 0 = Straight (Up), 1 = Full Bend (Down)
int repState = 0; 

// Straight calibrated dynamically during setup/reset
int STRAIGHT_THRESHOLD = 0; 
// Must get deep to count (Min bend is ~770)
const int FULL_REP_THRESHOLD = 540; 

// Function to handle auto-calibration sequence
void calibrateSensor() {
  long sum = 0;
  
  // Take 20 readings to get an average of the straight arm beginning position
  for (int i = 0; i < 20; i++) {
    sum += analogRead(flexPin);

    delay(50); 
  }
  
  // Calculate average and set threshold with a +40 margin for user "tolerance"
  int average = sum / 20;
  STRAIGHT_THRESHOLD = average + 40; 
  
  Serial.print("Calibration Complete. New Straight Threshold: ");
  Serial.println(STRAIGHT_THRESHOLD);
}

void setup() {
  // Start serial for tracking 
  Serial.begin(9600);
  
  // Enable pull-up for reset button
  pinMode(buttonPin, INPUT_PULLUP);

  // DISPLAY CONFIGURATION
  byte numDigits = 3; 
  bool resistorsOnSegments = true; 
  // Change to COMMON_ANODE if display is dark
  byte hardwareConfig = COMMON_CATHODE; 
  bool updateWithDelays = false; 
  bool leadingZeros = false; 
  bool disableDecPoint = true; 

  // Initialize display library
  sevseg.begin(hardwareConfig, numDigits, digitPins, segmentPins, resistorsOnSegments, updateWithDelays, leadingZeros, disableDecPoint);
  
  // Set brightness to visible level 
  sevseg.setBrightness(90);

  // Run initial calibration immediately
  calibrateSensor();
}

void loop() {
  sevseg.refreshDisplay(); 

  // Read current sensor value from elbow
  int flexValue = analogRead(flexPin);

  // Reset Button Logic
  if (digitalRead(buttonPin) == LOW) {
    pushupCount = 0;
    // Start fresh at beginning
    repState = 0; 
    
    // Re-calibrate whenever the button is pressed
    calibrateSensor();
  }

  // 1. Detect downward motion
  // If we reach the bottom threshold and were previously up, enter first state
  if (flexValue >= FULL_REP_THRESHOLD && repState == 0) {
    repState = 1; 
  } 
  
  // 2. Detect return to neutral
  // If we return to straight zone (within +40 of neutral) and were previously down, increment count of pushups
  if (flexValue <= STRAIGHT_THRESHOLD && repState == 1) {
    pushupCount++; 
    
    // RE-LOCK the state back to 0 until the user bends again
    repState = 0; 
  }

  Serial.println(flexValue);
  // Update 3-digit display with current count
  sevseg.setNumber(pushupCount);
}