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

const int MPU_ADDR = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ;

Servo servoLeft;
Servo servoRight;

// --- CONTROL VARIABLES ---
int flatAngle = 90;    // The neutral, horizontal position for the servos
int deadzone = 2000;   // Ignores minor movements to prevent servo jitter

void setup() {
  // Initialize I2C communication
  Wire.begin();
  
  // Wake up the MPU-6050 (starts in sleep mode)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Write 0 to wake it up
  Wire.endTransmission(true);

  // Attach servos to PWM pins
  servoLeft.attach(9);
  servoRight.attach(10);
  
  Serial.begin(9600);
  Serial.println("SIDS Cradle Prototype Initialized.");
}

void loop() {
  // Request accelerometer data from the MPU-6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);  // Read 6 registers total

  // Combine the high and low bytes for each axis
  AcX = Wire.read() << 8 | Wire.read();  
  AcY = Wire.read() << 8 | Wire.read();  
  AcZ = Wire.read() << 8 | Wire.read();  

  // ==========================================
  // --- Z-AXIS ORIENTATION DETECTION ---
  // ==========================================
  // The Z-axis measures gravity pointing straight down through the chip.
  
  Serial.print("[Z-Axis: ");
  if (AcZ > 8000) {
    Serial.print("Upright");
  } else if (AcZ < -8000) {
    Serial.print("UPSIDE DOWN!");
  } else {
    Serial.print("Tilted 90 Deg");
  }
  Serial.print("]  ->  ");

  // ==========================================
  // --- DUAL-ACTION V-SHAPE CONTROL LOGIC ---
  // ==========================================
  // This logic only uses the X-axis for rolling, actively ignoring the 
  // Z-axis to maintain a safe 1D mechanical mapping for the servos.

  if (AcX < -deadzone) {
    // === BABY ROLLS LEFT ===
    
    // Primary Action: Left side lifts aggressively to push back
    int leftPush = map(AcX, -deadzone, -16000, 90, 180);
    leftPush = constrain(leftPush, 90, 180); 
    
    // Secondary Action: Right side lifts to form the "Soft Cushion" wall
    int rightCushion = map(AcX, -deadzone, -16000, 90, 135);
    rightCushion = constrain(rightCushion, 90, 135);

    servoLeft.write(leftPush);        
    servoRight.write(rightCushion);  

    // Original verified print statements
    Serial.print("Rolling LEFT | Pushing Left: ");
    Serial.print(leftPush);
    Serial.print(" | Cushioning Right: ");
    Serial.println(rightCushion);

  } 
  else if (AcX > deadzone) {
    // === BABY ROLLS RIGHT ===
    
    // Primary Action: Right side lifts aggressively to push back
    int rightPush = map(AcX, deadzone, 16000, 90, 180); 
    rightPush = constrain(rightPush, 90, 180); 

    // Secondary Action: Left side lifts to form the "Soft Cushion" wall
    int leftCushion = map(AcX, deadzone, 16000, 90, 135);
    leftCushion = constrain(leftCushion, 90, 135);

    servoRight.write(rightPush);      
    servoLeft.write(leftCushion);   

    // Original verified print statements
    Serial.print("Rolling RIGHT | Pushing Right: ");
    Serial.print(rightPush);
    Serial.print(" | Cushioning Left: ");
    Serial.println(leftCushion);

  } 
  else {
    // === FLAT / SUPINE (Inside Deadzone) ===
    
    // Both servos return to the horizontal "prepared" state
    servoLeft.write(flatAngle);
    servoRight.write(flatAngle);
    
    // Original verified print statement
    Serial.println("FLAT | Both Servos Prepared (90)");
  }

  delay(20); // Small delay to prevent Serial spam and allow servos to catch up
}