#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

uint8_t CSPin[4] = {5, 4, 3, 2};
uint8_t LDRPin[4] = {A0, A1, A2, A3};

const float gamma = 0.7;
const float rl10 = 50;

Adafruit_MPU6050 mpu[4];
sensors_event_t accel[4], gyro[4], temp[4];

void setup() {
  Serial.begin(115200);
  
  for(uint8_t i = 0; i < 4; i++) {
    pinMode(CSPin[i], OUTPUT);
  }

  for(uint8_t i = 0; i < 4; i++) {
    switch(i) {
      case 0:
        digitalWrite(CSPin[0], LOW);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 1:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], LOW);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 2:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], LOW);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 3:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], LOW);
        break;

      default:
        break;
    }

    if(!mpu[i].begin()) {
      Serial.print("Failed to initialize MPU ");
      Serial.println(i);
      while(1) {
        delay(1000);
      }
    }
    Serial.print("MPU ");
    Serial.print(i);
    Serial.println(" found");

    mpu[i].setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu[i].setGyroRange(MPU6050_RANGE_500_DEG);
    mpu[i].setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  Serial.println("All MPU ready");
  delay(1000);
}

void loop() {
  for(uint8_t i = 0; i < 4; i++) {
    switch(i) {
      case 0:
        digitalWrite(CSPin[0], LOW);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 1:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], LOW);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 2:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], LOW);
        digitalWrite(CSPin[3], HIGH);
        break;

      case 3:
        digitalWrite(CSPin[0], HIGH);
        digitalWrite(CSPin[1], HIGH);
        digitalWrite(CSPin[2], HIGH);
        digitalWrite(CSPin[3], LOW);
        break;

      default:
        break;
    }

    mpu[i].getEvent(&accel[i], &gyro[i], &temp[i]);
    int analogValue = analogRead(LDRPin[i]);
    float voltage = analogValue / 1024. * 5;
    float resistance = 2000 * voltage / (1 - voltage / 5);
    float lux = pow(rl10 * 1e3 * pow(10, gamma) / resistance, (1 / gamma));

    Serial.print("Acceleration X: ");
    Serial.print(accel[i].acceleration.x);
    Serial.print(", Y: ");
    Serial.print(accel[i].acceleration.y);
    Serial.print(", Z: ");
    Serial.print(accel[i].acceleration.z);
    Serial.println(" m/s^2");

    Serial.print("Rotation X: ");
    Serial.print(gyro[i].gyro.x);
    Serial.print(", Y: ");
    Serial.print(gyro[i].gyro.y);
    Serial.print(", Z: ");
    Serial.print(gyro[i].gyro.z);
    Serial.println(" rad/s");

    Serial.print("Temperature: ");
    Serial.print(temp[i].temperature);
    Serial.println(" degC");

    Serial.print("Light intensity: ");
    Serial.print(lux);
    Serial.println("lux");
  }
  Serial.println();
  delay(1000);
}