// Ethan Hansen
// Intro to Computing - Arduino Project
// Project 3- Ultrasonic Safety Monitoring System
/* PROGRAM:
    1. READ DISTANCE -Read 10 distance readings from Ultronsic Distance Sensor
    2. CALCULATE AVERAGE -Calculate average of 10 readings
    3. SET STATUS - Determine Status from avg dist: SAFE, WARNING, or DANGER
    4. LIGHT LEDs -Light LED array based on Status
    5. MOVE MOTOR -Move stepper motor based on Status
    6. FEEDBACK -Ouput status to Serial Monitor
*/
//-------------------------------------------------------------------------------------

//-----------------------Subroutine Declarations------------------------
#include <stdio.h>
#include <Stepper.h>
void read_dist(int *distarrayptr);
void avg_dist(int *distarrayptr, float *avg);
void led_status(float d, int *led, int *spd); 
void light_leds(int led);
void moveMotor(int spd); 
void displayStatus(int led); 


//------------------------------SETUP-----------------------------------
//Global Variables:
const int trigPin= 12, echoPin= 13; //ultrasonic sensor pins
const int leds[6]= {2,3,4,5,6,7}; //led pins array
const int stepsPerRev=200; //200 steps = 1 Revolution 
Stepper myStepper(stepsPerRev,8,9,10,11); //Global stepper object, motor on pins 8-11

void setup() {
  Serial.begin(9600); //start serial monitor...
  pinMode(trigPin,OUTPUT); //ultrasonic sensor pins
  pinMode(echoPin,INPUT); 
  for(int i=2; i<=7; i++) //LED pins
    pinMode(i,OUTPUT);
}

//---------------------------MAIN ROUTINE--------------------------------
void loop() {
 
  //variables + arrays:
  int readings[10]; //distance values
  float avg; //average distance
  int ledCount, mtrSpeed; //# of leds to turn on + motor speed

  //MAIN FUNCTION CALLS:
  read_dist(&readings[0]);              //1. READ DISTANCE 
  avg_dist(&readings[0],&avg);          //2. CALCULATE AVERAGE 
  led_status(avg, &ledCount, &mtrSpeed);//3. SET STATUS
  light_leds(ledCount);                 //4. LIGHT LEDs
  moveMotor(mtrSpeed);                  //5. MOVE MOTOR
  displayStatus(ledCount);              //6. FEEDBACK
}

//--------------------------SUBROUTINES-----------------------------------

//1. READ DISTANCE
//INPUT: -
//RETURN: distance values to readings[10] (using ptr)
void read_dist(int *distarrayptr)
{
  for(int i=0; i<=9; i++)
  {
    digitalWrite(trigPin, LOW); //makes sure trigPin is OFF
    delayMicroseconds(2);

    //send out sound for 10 microseconds
    digitalWrite(trigPin, HIGH); 
    delayMicroseconds(10);
    digitalWrite(trigPin,LOW);

    //count time till echo
    long t = pulseIn(echoPin,HIGH);
  
    //convert to distance and store in readings[10] w. ptr
    distarrayptr[i] = t*0.034/2; //formula for time -> distance (in cm)
  }
}

//2. CALCULATE AVERAGE
//INPUT: readings[10]
//RETURN: average
void avg_dist(int *distarrayptr, float *avg)
{
  int sum=0;
  for(int i=0; i<=9; i++) //sum values in array
  {
    sum = sum + distarrayptr[i];
  }
  *avg = sum/10.0; //divide by 10 + return to average in loop() using ptr
}

//3. SET STATUS
//INPUT: average
//RETURN: LED count + motor speed (using ptrs)
void led_status(float d, int *led, int *spd)
{
  if (300<=d) //SAFE Status (300+ cm away)
  {
    *led = 1; //1 led on
    *spd = 10; //10 RPM
  }
  else if (150<=d) //WARNING Status (150-299 cm away)
  {
    *led = 3; //3 leds on
    *spd = 45; //45 RPM
  }
  else //DANGER Status (less than 150 cm away)
  {
    *led = 6; //6 leds on
    *spd = 90; //90 RPM
  }
}

//4. LIGHT LEDS 
//INPUT: ledCount
//RETURN: -
void light_leds(int led)
{
  for (int i=0; i<=5; i++) //turn all LEDs off before relighting them
    digitalWrite(leds[i],LOW);

  for (int i=0; i<=led-1; i++) //turn on LEDs 1 through ledCount
    digitalWrite(leds[i],HIGH);
}

//5. MOVE MOTOR
//INPUT: mtrSpeed
//RETURN: -
void moveMotor(int spd)
{
  myStepper.setSpeed(spd); //set speed to mtrSpeed
  
  if (spd<=45) //SAFE + WARNING status:
    myStepper.step(stepsPerRev); //forward 1 rev
  else //DANGER status:
    myStepper.step(-2*stepsPerRev); //reverse 2 rev
}

//6. FEEDBACK
//INPUT: ledCount (conveys system status)
//OUTPUT: -
void displayStatus(int led)
{
  switch(led)
  {
    case 1: //1 led = SAFE status
      Serial.println("System Status: SAFE");
      break;
    case 3: //3 leds = WARNING status 
      Serial.println("System Status: WARNING");
      break;
    case 6: //6 leds = DANGER status
      Serial.println("System Status: DANGER!"); 
      break;
    default: //shouldn't run
      Serial.println("Uh oh. somethings wrong...");
  }
}