/*
  Wokwi | projects
**Project Link:** https://wokwi.com/projects/449133592823789569?gh=1
*/

#include <Adafruit_MPU6050.h>
#include <LiquidCrystal.h>
#include <math.h>

#define ROLL_MATRIX_ROWS    3 // rows of the roll matrix
#define ROLL_MATRIX_COLS    3 // columns of the roll matrix
#define PITCH_MATRIX_ROWS   3 // rows of the pitch matrix
#define PITCH_MATRIX_COLS   3 // columns of the pitch matrix
#define YAW_MATRIX_ROWS     3 // rows of the yaw matrix
#define YAW_MATRIX_COLS     3 // columns of the yaw matrix
#define START_VECTOR_ROWS   3 // rows of the start vector
#define START_VECTOR_COLS   1 // columns of the start vector
#define POS_VECTOR_ROWS     3 // rows of the postion vector
#define POS_VECTOR_COLS     1 // columns of the position vector

const int ECHO_PIN = 2;
const int TRIG_PIN = 3;

sensors_event_t event; // global declaration of event as a variable

Adafruit_MPU6050 mpu;
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

float getDistance() {
  // send trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // calculate distance
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * .0345) / 2;
  return distance;
}

void setup()  {
  Serial.begin(115200);
  lcd.begin(16, 2);
  if (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    while (1);
  }
  //Serial.println("MPU6050 ready!");
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.println("System ready\n");
}

void loop() {
  // read gyro
  mpu.getGyroSensor()->getEvent(&event);
  float x = event.gyro.x ; //declaring variables
  float y = event.gyro.y ; //declaring variables
  float z = event.gyro.z ; //declaring variables
  // print rotation measurements
  Serial.println("Rotation in rad/s:");
  Serial.print("X: ");
  Serial.print(x);
  Serial.print(", Y: ");
  Serial.print(y);
  Serial.print(", Z: ");
  Serial.println(z);

  float distance = getDistance();
  // print distance
  Serial.print("Distance in cm: ");
  Serial.println(distance);

  int Roll[ROLL_MATRIX_ROWS][ROLL_MATRIX_COLS] =
  { //roll matrix
    {cos(y), 0, sin(y)},
    {0, 1, 0},
    {-sin(y), 0, cos(y)}
  };

  int Pitch[PITCH_MATRIX_ROWS][PITCH_MATRIX_COLS] =
  { //pitch marix
    {1, 0, 0},
    {0, cos(x), -sin(x)},
    {0, sin(x), cos(x)}
  };

  int Yaw[YAW_MATRIX_ROWS][YAW_MATRIX_COLS] =
  { //yaw matrix
    {cos(z), -sin(z), 0},
    {sin(z), cos(z), 0},
    {0, 0, 1}
  };

  int startVector[START_VECTOR_ROWS][START_VECTOR_COLS] =
  {
    {distance},
    {0},
    {0}
  };

  // The multiplication
  int firstresult[ROLL_MATRIX_ROWS][PITCH_MATRIX_COLS] = {0}; // Resultant matrix initialized to 0
  // Matrix multiplication
  for (int i = 0; i < ROLL_MATRIX_ROWS; i++)
  {
    for (int j = 0; j < PITCH_MATRIX_COLS; j++) {
      for (int k = 0; k < ROLL_MATRIX_COLS; k++) {
        firstresult[i][j] += Roll[i][k] * Pitch[k][j];
      }
    }
  }

  int result[ROLL_MATRIX_ROWS][PITCH_MATRIX_COLS] = {0}; // Resultant matrix initialized to 0
  for (int i = 0; i < ROLL_MATRIX_ROWS; i++)
  {
    for (int j = 0; j < PITCH_MATRIX_COLS; j++) {
      for (int k = 0; k < ROLL_MATRIX_COLS; k++) {
        result[i][j] += firstresult[i][k] * Yaw[k][j];
      }
    }
  }

  int P[POS_VECTOR_ROWS][POS_VECTOR_COLS] = {0}; // Resultant matrix initialized to 0
  for (int i = 0; i < ROLL_MATRIX_ROWS; i++)
  {
    for (int j = 0; j < PITCH_MATRIX_COLS; j++) {
      for (int k = 0; k < ROLL_MATRIX_COLS; k++) {
        P[i][j] += result[i][k] * startVector[k][j];
      }
    }
  }

  lcd.setCursor(0, 0);
  lcd.print("Pos: ");
  Serial.println("Pos:");
  for (int i = 0; i < ROLL_MATRIX_ROWS; i++) {
    for (int j = 0; j < POS_VECTOR_COLS; j++) {
      lcd.print(P[i][j]);
      Serial.println(P[i][j]);
    }
  }
  Serial.println();

  delay(2000);

}