#include <Adafruit_ADXL345_U.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>


const int led2pin = 3;
const int led3pin = 5;
const int led4pin = 6;
const int bpmMax = 220;
const int bpmMin = 70;




int brightness = 255;
int fadeAmount = 10;

long elapsed_time = 0;
long previousMillis = 0;
long previousMillis2 = 0;
long servointerval = 15;
long interval = 1000;
int pos = 0;

int Xaccel = 0;
int Yaccel = 0;
int Zaccel = 0;

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards



/* Assign a unique ID to this sensor at the same time */
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);

void displaySensorDetails(void)
{
  sensor_t sensor;
  accel.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" m/s^2");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" m/s^2");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" m/s^2");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

void displayDataRate(void)
{
  Serial.print  ("Data Rate:    ");

  switch (accel.getDataRate())
  {
    case ADXL345_DATARATE_3200_HZ:
      Serial.print  ("3200 ");
      break;
    case ADXL345_DATARATE_1600_HZ:
      Serial.print  ("1600 ");
      break;
    case ADXL345_DATARATE_800_HZ:
      Serial.print  ("800 ");
      break;
    case ADXL345_DATARATE_400_HZ:
      Serial.print  ("400 ");
      break;
    case ADXL345_DATARATE_200_HZ:
      Serial.print  ("200 ");
      break;
    case ADXL345_DATARATE_100_HZ:
      Serial.print  ("100 ");
      break;
    case ADXL345_DATARATE_50_HZ:
      Serial.print  ("50 ");
      break;
    case ADXL345_DATARATE_25_HZ:
      Serial.print  ("25 ");
      break;
    case ADXL345_DATARATE_12_5_HZ:
      Serial.print  ("12.5 ");
      break;
    case ADXL345_DATARATE_6_25HZ:
      Serial.print  ("6.25 ");
      break;
    case ADXL345_DATARATE_3_13_HZ:
      Serial.print  ("3.13 ");
      break;
    case ADXL345_DATARATE_1_56_HZ:
      Serial.print  ("1.56 ");
      break;
    case ADXL345_DATARATE_0_78_HZ:
      Serial.print  ("0.78 ");
      break;
    case ADXL345_DATARATE_0_39_HZ:
      Serial.print  ("0.39 ");
      break;
    case ADXL345_DATARATE_0_20_HZ:
      Serial.print  ("0.20 ");
      break;
    case ADXL345_DATARATE_0_10_HZ:
      Serial.print  ("0.10 ");
      break;
    default:
      Serial.print  ("???? ");
      break;
  }
  Serial.println(" Hz");
}

void displayRange(void)
{
  Serial.print  ("Range:         +/- ");

  switch (accel.getRange())
  {
    case ADXL345_RANGE_16_G:
      Serial.print  ("16 ");
      break;
    case ADXL345_RANGE_8_G:
      Serial.print  ("8 ");
      break;
    case ADXL345_RANGE_4_G:
      Serial.print  ("4 ");
      break;
    case ADXL345_RANGE_2_G:
      Serial.print  ("2 ");
      break;
    default:
      Serial.print  ("?? ");
      break;
  }
  Serial.println(" g");
}

void setup() {

  pinMode(led2pin, OUTPUT);
  pinMode(led3pin, OUTPUT);
  pinMode(led4pin, OUTPUT);
  Serial.begin(9600);
  Serial.println("Accelerometer Test"); Serial.println("");
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  /* Initialise the sensor */
  if (!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
    while (1);
  }

  /* Set the range to whatever is appropriate for your project */
  accel.setRange(ADXL345_RANGE_16_G);
  // displaySetRange(ADXL345_RANGE_8_G);
  // displaySetRange(ADXL345_RANGE_4_G);
  // displaySetRange(ADXL345_RANGE_2_G);

  /* Display some basic information on this sensor */
  displaySensorDetails();

  /* Display additional settings (outside the scope of sensor_t) */
  displayDataRate();
  displayRange();
  Serial.println("");


}

void loop() {
  // put your main code here, to run repeatedly:

  analogWrite(led2pin, brightness);
  analogWrite(led3pin, brightness);
  analogWrite(led4pin, brightness);
  
  /* Get a new sensor event */
  sensors_event_t event;
  accel.getEvent(&event);

  /* Display the results (acceleration is measured in m/s^2) */
  Serial.print("X: "); Serial.print(event.acceleration.x); Serial.print("  ");
  Serial.print("Y: "); Serial.print(event.acceleration.y); Serial.print("  ");
  Serial.print("Z: "); Serial.print(event.acceleration.z); Serial.print("  "); Serial.println("m/s^2 ");
  
  Xaccel = event.acceleration.x;
  Yaccel = event.acceleration.y;
  Zaccel = event.acceleration.z;

  Serial.print("Xaccel: ");
  Serial.print(abs(Xaccel));

  if (abs(Xaccel) > 3 || abs(Zaccel) > 5) {
    if (brightness == 255){
      brightness = 0;
    }
    else{
      brightness = 255;
    }
     for (pos = 0; pos <= 90; pos += 3) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
    for (pos = 90; pos >= 0; pos -= 3) { // goes from 180 degrees to 0 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
 
    delay(100);
  }
  else if (abs(Xaccel) <= 6 || abs(Zaccel) <= 7) {
    if (brightness == 255){
      brightness = 0;
    }
    else{
      brightness = 255;
    }
      for (pos = 0; pos <= 90; pos += 3) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
    for (pos = 90; pos >= 0; pos -= 3) { // goes from 180 degrees to 0 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
 delay(400);
  }
  else if (abs(Xaccel) >=8 || abs(Zaccel) > 9){
    if (brightness == 255){
      brightness = 0;
    }
    else{
      brightness = 255;
    }
      for (pos = 0; pos <= 90; pos += 3) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
    for (pos = 90; pos >= 0; pos -= 3) { // goes from 180 degrees to 0 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      //delay(1);// waits 15ms for the servo to reach the position
    }
    delay(600);
  }
  else {
    if (brightness == 255){
      brightness = 0;
    }
    else{
      brightness = 255;
    }
    for(pos = 0; pos < 60; pos+=60) {      //opens the arm 60 degrees
       unsigned long servoMillis = millis();
      if(servoMillis - previousMillis > servointerval)  //created to delay 15 milliseconds before next pulse
        {
         previousMillis = servoMillis;
         myservo.write(pos);      //moves 5 degrees every 15 milliseconds
        }
     
     }
       
      unsigned long Starttimer=millis();      //starts one second delay timer?
         do {
            unsigned long current_time = millis();
            elapsed_time = current_time - Starttimer;
            }
            while(elapsed_time < interval);      //leaves loop when 1 second has passed
      
      
      for(pos = 60; pos >=60; pos-=60) {    //closes the arm back to 0 degrees
        unsigned long servoMillis2 = millis();
        if(servoMillis2 - previousMillis2 > servointerval)
        {
        previousMillis2 = servoMillis2;
        myservo.write(pos);
        }
       
    }
       

    delay(15);
  }



}