//MPU Gyroscope Libraries
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <util/delay.h>

//Ultasonik sensör pin girişleri (echo ve trigger)
#define PIN_TRIG 3
#define PIN_ECHO 2


//Fefine the mpu as mpu
Adafruit_MPU6050 mpu;
#define MPU_LED 5

//MPU5060 Event
sensors_event_t event;

//First stepper motor test pins
#define stepPin 31
#define dirPin 33

#define calibrator 29.6

int currentStep = 0;
int rotateStep = 0;

void setup() {
  //Communication rate
  Serial.begin(115200);
  //Ultrasonic setup
  DDRE |= 0B100000;
  

  //stepper sag arka
  DDRC |= 0B1010000;
  //stepper sag on
  DDRH |= 0B1100000;
  //Stepper sol on
  DDRB |= 0B110000;
  //Stepper sol arka
  DDRL |= 0B1010;
  

  // Try to initialize the mpu sensor
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 initialized succesfully");
  DDRE |= 0B1000;
  PORTE |= (1<<3);
  
  
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  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;
  }

  //Stepper alignment
  PORTC |= (1<<4);
  PORTC |= (1<<6);
  PORTL |= (1<<1);
  PORTL |= (1<<3);
  PORTH |= (1<<5);
  PORTH |= (1<<6);
  PORTB |= (1<<4);
  PORTB |= (1<<5);
  
  delayMicroseconds(2000); 
  PORTC &= ~(1<<6);
  PORTL &= ~(1<<3);
  PORTB &= ~(1<<5);
  PORTH &= ~(1<<6);
  PORTC |= (1<<6);
  PORTH |= (1<<6);
  PORTB |= (1<<5);
  PORTL |= (1<<3);

  delayMicroseconds(2000); 
  PORTC &= ~(1<<6);
  PORTH &= ~(1<<6);
  PORTB &= ~(1<<5);
  PORTL &= ~(1<<3);
  delayMicroseconds(2000);

  

}

// Function to measure the pulse duration in microseconds
unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout) {
    // Cache the port and bit of the pin for faster resolution
    uint8_t bit = 0B10000;
    int port = 5;
    uint8_t stateMask = (state ? bit : 0);
    unsigned long width = 0;

    // Convert the timeout from microseconds to clock cycles
    unsigned long numloops = 0;
    unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;

    // Wait for any previous pulse to end
    while ((*portInputRegister(port) & bit) == stateMask) {
        if (numloops++ == maxloops)
            return 0;
    }

    // Wait for the pulse to start
    while ((*portInputRegister(port) & bit) != stateMask) {
        if (numloops++ == maxloops)
            return 0;
    }

    // Wait for the pulse to stop
    while ((*portInputRegister(port) & bit) == stateMask) {
        if (numloops++ == maxloops)
            return 0;
        width++;
    }

    // Convert the reading to microseconds
    // The loop has been determined to be 20 clock cycles long
    // with about 16 clocks between the edge and the start of the loop
    return width;
}



float distancefinder() {
    PORTE |= (1 << 5); // Set trigger pin HIGH
    delayMicroseconds(10);
    PORTE &= ~(1 << 5); // Set trigger pin LOW

    int duration = pulseIn(PIN_ECHO, HIGH, 30000); // Timeout set to 30 ms (max range)
    
    return duration / calibrator; // Convert duration to distance in centimeters
}

int stepperTest(int distanceData){
  
  int stepCount = 0;
  //Printing data for convenience
  Serial.print("Distancedata: ");
  Serial.println(distanceData);
  Serial.print("CurrentStep: ");
  Serial.println(currentStep);
    if(currentStep < distanceData){
      Serial.println("currentStep < distanceData");
      // Enables the motor to move in a particular direction
      PORTL |= (1<<1);
      PORTB |= (1<<4);
      PORTC |= (1<<4);
      PORTH |= (1<<5);
      for(int x = 0; x < distanceData; x++) { 
        PORTC |= (1<<6);
        PORTH |= (1<<6);
        PORTB |= (1<<5);
        PORTL |= (1<<3);
        delayMicroseconds(2000); 
        PORTC &= ~(1<<6);
        PORTH &= ~(1<<6);
        PORTB &= ~(1<<5);
        PORTL &= ~(1<<3);
        delayMicroseconds(2000); 
        stepCount++;
      } 
      currentStep = currentStep + stepCount;
      stepCount = 0;
  }
  else if(currentStep > distanceData)
  {
    
      PORTL &= ~(1<<1);
      PORTB &= ~(1<<4);
      PORTC &= ~(1<<4);
      PORTH &= ~(1<<5);
      for(int x = 0;  x < currentStep - distanceData; x++) {
      PORTC |= (1<<6);
      PORTH |= (1<<6);
      PORTB |= (1<<5);
      PORTL |= (1<<3);
      _delay_us(2000);
      PORTC &= ~(1<<6);
      PORTH &= ~(1<<6);
      PORTB &= ~(1<<5);
      PORTL &= ~(1<<3);
      _delay_us(2000);
      stepCount++;
      } 
      currentStep = currentStep - stepCount;
      stepCount = 0;

  }

    
    //delay(1000); // One second delay
  return currentStep;
}

void controlStepperX(int steps) {
  if (steps > 0) {
    PORTH |= (1 << 5); //Sag ön artacak
    PORTC |= (1 << 4); //Sag arka artacak
    PORTB &= ~(1 << 4); //Sol on azal
    PORTL &= ~(1 << 1); //SOL arka azal
  } else {
    PORTH &= ~(1 << 5); //Sag ön azalacak
    PORTC &= ~(1 << 4); //Sag arka azalacak
    PORTB |= (1 << 4); //Sol on art
    PORTL |= (1 << 1); //SOL arka art
  }

  steps = abs(steps);
  for (int i = 0; i < steps; i++) {
    PORTC |= (1<<6);
    PORTH |= (1<<6);
    PORTB |= (1<<5);
    PORTL |= (1<<3);
    _delay_us(2000);          // Wait 1 millisecond
    PORTC &= ~(1<<6);
    PORTH &= ~(1<<6);
    PORTB &= ~(1<<5);
    PORTL &= ~(1<<3);
    _delay_us(2000);          // Wait 1 millisecond
  }
}

void controlStepperY(int steps) {
  if (steps > 0) {
    PORTH &= ~(1 << 5); //Sag ön azalacak
    PORTB &= ~(1 << 4); //Sol on azal
    PORTC |= (1 << 4); //Sag arka artacak
    PORTL |= (1 << 1); //SOL arka artacak
  } else {
    PORTH |= (1 << 5); //Sag ön artacak
    PORTB |= (1 << 4); //Sol on artacak
    PORTC &= ~(1 << 4); //Sag arka azalacak
    PORTL &= ~(1 << 1); //SOL arka azalacak
  }

  steps = abs(steps);
  for (int i = 0; i < steps; i++) {
    PORTC |= (1<<6);
    PORTH |= (1<<6);
    PORTB |= (1<<5);
    PORTL |= (1<<3);
    _delay_us(2000);          // Wait 1 millisecond
    PORTC &= ~(1<<6);
    PORTH &= ~(1<<6);
    PORTB &= ~(1<<5);
    PORTL &= ~(1<<3);
    _delay_us(2000);          // Wait 1 millisecond
  }
}

void loop() {
  // Start a new measurement:
  int distance = distancefinder();
  Serial.println(distance);
  //Get sensor event for acceleration, rotation and temperature
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.println("");
  
  stepperTest(distance);
  int stepsX = (int)(g.gyro.x * 10); 
  int stepsY = (int)(g.gyro.y * 10); 
  controlStepperX(stepsX);
  controlStepperY(stepsY);
  

  delay(500);
}