#include <Wire.h>

// Use the Arduino Digital labels from your diagram
#define STEP_PIN D2
#define DIR_PIN  D3

#define MPU_ADDR 0x68
#define BMP_ADDR 0x77

long curPos = 0;
int dir = 1;
unsigned long lastStep = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);

  // Tell the STM32 to use the pins labeled D14 and D15 for I2C
  Wire.setSDA(PB7); // PB7 is D14
  Wire.setSCL(PB6); // PB6 is D15
  Wire.begin();

  // Wake up MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); 
  Wire.write(0);    
  if (Wire.endTransmission() != 0) {
    Serial.println("I2C Error: Check Wiring!");
  }
}

void loop() {
  // Stepper Logic
  if (micros() - lastStep >= 2000) {
    lastStep = micros();
    digitalWrite(DIR_PIN, dir > 0 ? HIGH : LOW);
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(2);
    digitalWrite(STEP_PIN, LOW);
    
    curPos += dir;
    if (curPos >= 200) dir = -1;
    else if (curPos <= -200) dir = 1;
  }

  // Sensor Logic (Every 1 second)
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate >= 1000) {
    lastUpdate = millis();

    // Read MPU6050
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_ADDR, 2);
    int16_t ax = (Wire.read() << 8) | Wire.read();

    // Read BMP180
    Wire.beginTransmission(BMP_ADDR);
    Wire.write(0xF4);
    Wire.write(0x34); 
    Wire.endTransmission();
    delay(20); 
    Wire.beginTransmission(BMP_ADDR);
    Wire.write(0xF6);
    Wire.endTransmission(false);
    Wire.requestFrom(BMP_ADDR, 2);
    uint16_t p = (Wire.read() << 8) | Wire.read();

    Serial.print("X:"); Serial.print(ax);
    Serial.print(" | P:"); Serial.print(p);
    Serial.print(" | S:"); Serial.println(curPos);
  }
}