#include <MD_MAX72xx.h>
#include <SPI.h> //SPI communication
//mpu6050 libraries
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

#define  delay_t  50  // in milliseconds //For settling delays in the program

#define HARDWARE_TYPE MD_MAX72XX::FC16_HW 
#define MAX_DEVICES 4 
//Adjustment 1
//#define MAX_DEVICES 1 //Maximum number of devices (LED matrix modules) connected ('MAX_DEVICCES')


Adafruit_MPU6050 mpu; //Creates an object named 'mpu' of type Adafruit_MPU6050, which will be used to interface with the MPU6050 sensor.

//buzzer pin
int buzzerPin = 7; //Initialize the buzzer pin

//ultrasonic sensor pins 
#define ECHO_PIN 3 //echo signal
#define TRIG_PIN 4 //trigger signal

//max7219 dotmatrix display pins
#define CLK_PIN   52 //Clock
#define DATA_PIN  51 //Data
#define CS_PIN      53  //Chip select

//analog joystick pins
#define VERT_PIN A0 //vertical axis
#define HORZ_PIN A1 //horizontal axis
#define SEL_PIN  2 //selection button

// Hardware SPI connection
MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, CS_PIN, MAX_DEVICES); //Initializes an instance of the 'MD_MAX72XX' class named 'mx', used to ocntrol the MAX7219 LED matrix modules.
//MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, CS_PIN, 1);
//It specifies the hardware type, chip select pin, maximum number of devices.


//LED Pattern
byte rightarrow[8] = {0x10,0x30,0x70,0xFF,0xFF,0x70,0x30,0x10};
byte leftarrow[8] = {0x08,0x0C,0x0E,0xFF,0xFF,0x0E,0x0C,0x08};
byte stopsign[8] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};

void setup() {  

  Serial.begin(115200); 
  while (!Serial) //loop waits until the serial connection is established, ensuring that the program dodesnt proceed until communication is available.
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Initialize communication with the MPU6050 sensor. If the initialization fails.....
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G); // Configures the accelerometer range of the MPU6050 sensor to+/- 8G and prints the selected range to the serial monitor.
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG); // Defines the range for gyroscope
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); //sets the filter bandwidth of the MPU6050 sensor to 21Hz and prints the selected bandwidth to the serial monitor
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println(""); 
  delay(100);

  mx.begin(); //Initializes the LED matrix display 'mx/begin()', 
  //mx.control(MD_MAX72XX::INTENSITY, 0);
  //Adjustment 2
  mx.control(MD_MAX72XX::INTENSITY, 1);
  mx.clear(); // clears the display

  pinMode(buzzerPin, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  }

  float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW); //Sets the trigger pin (TRIG_PIN) to a low state to ensure a clean pulse.
  delayMicroseconds(2); //Waits for 2 microseconds.
  digitalWrite(TRIG_PIN, HIGH); //Sets the trigger pin to a high state to send out an ultrasonic pulse.
  delayMicroseconds(10); //Waits for 10 microseconds.
  digitalWrite(TRIG_PIN, LOW); //Sets the trigger pin back to a low state after sending the pulse.
  int duration = pulseIn(ECHO_PIN, HIGH); //Measures the duration it takes for the ultrasonic pulse to 
  //return by using the pulseIn() function. It waits until the echo pin (ECHO_PIN) becomes HIGH and then measures the duration.
  return duration * 0.034 / 2; //Calculates the distance based on the duration of the pulse
//the speed of sound is approximately 0.034 cm/microsecond
  
}

void loop() {

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp); // Retrieves sensor data from the MPU6050 accelerometer and gyroscope.

// JOYSTICK & TURN SIGNALS
  int horz = analogRead(HORZ_PIN); // Reads the horizontal axis analog input from the joystick.
  int vert = analogRead(VERT_PIN); //Reads the vertical axis analog input from the joystick.
  if (horz < 300) {
    drawShape();
  }
  if (horz > 700) {
    drawShape2();
  }
  if (digitalRead(SEL_PIN) == LOW) {
    mx.clear();
  }
  //drawShape();


// ULTRASONIC SENSOR & BUZZER
  float distance = readDistanceCM(); //Calls the readDistanceCM() function to measure the 
  //distance using the ultrasonic sensor and stores the result in the distance variable.

  if (distance < 100)
  {
     tone(buzzerPin, 100, 1000); //activates the buzzer
     delay(2000);
  }
  Serial.print("Measured distance: ");
  Serial.println(readDistanceCM());


  // ACCELERTOMETER
  if(a.acceleration.x==0)
  {
    drawshape3();
  }
}



void drawShape() {
  
  for (int i = 0; i <= 7; i++) {
    mx.setRow(3, 3, i, rightarrow[i]);
  }
  delay(delay_t);
}

void drawShape2() {
  
  for (int i = 0; i <= 7; i++) {
    mx.setRow(3, 3, i, leftarrow[i]);
  }
  delay(delay_t);
}

void drawshape3() {
  
  for (int i = 0; i <= 7; i++) {
    mx.setRow(3, 3, i, stopsign[i]);
  }
  delay(delay_t);
}