#include "Wire.h"
    #include "I2Cdev.h"
    #include "MPU6050.h"
    #include "Servo.h"
  
  /*This code was made to function with a custom-made Gimball with an attached DSLR-camera. It's made to
     *function with a 9-axis MPU motion-sensor, equipped with a gyroscope, an accelerometer, and a compass.*/
  
  //Set up some pins.
  int joystickVerticalPin = A0;      
  int joystickHorizontalPin = A1;
  int focusButton = 2;
  int shutterButton = 3;
  int focusPin = 4;
  int shutterPin = 5;
  int joyButtonPin = 6;
  
  MPU6050 mpu;                     //The MPU6050-object is a reference to our 9-axis gyroscope.
  
  Servo servoPitch;                //This servo-object refers to the servo responsible for the pitch-angle.
  Servo servoRoll;                 //This servo-object refers to the servo responsible for the roll-angle.
  Servo servoYaw;                  //This servo-object refers to the servo responsible for the yaw-angle.
  
  int pitchPrevious;               //Stores the pitch-angle from the previous loop for reference in the next iteration.
  int rollPrevious;                //Stores the roll-angle from the previous loop for reference in the next iteration.
  int yawPrevious;                 //Stores the yaw-angle from the previous loop for reference in the next iteration.

  const int maxPitch = 140;        //Limits the maximum pitch-angle.  
  const int maxRoll = 140;         //Limits the maximum roll-angle.  
  const int maxYaw = 100;          //Limits the maximum yaw-angle.
  const int minPitch = 58;         //Limits the minimum pitch-angle. 
  const int minRoll = 63;          //Limits the minimum roll-angle. 
  const int minYaw = 10;           //Limits the minimum yaw-angle.
  const int zeroPitch = 105;       //Neutral pitch.
  const int zeroRoll = 102;        //Neutral roll.
  const int zeroYaw = 58;          //Neutral yaw.
  
  boolean yawLocked = false;       //True if the yaw-rotation is deactivated.
  
  const float percentGyro = 0.90;  //The percentage of the gyro-signal that is used in the complimentary filter computations.
  const float percentAcc = 0.10;   //The percentage of the accelerometer-signal that is used in the complimentary filter computations.
    
  float joystickVertical = 0;      //This holds the additional pitch-angle caused by joystick-tilt.
  float joystickHorizontal = 0;    //This holds the additional roll-angle caused by joystick-pan.
  
  unsigned long focusTimer;        //Timer to know when to stop the focus-signal.
  unsigned long shutterTimer;      //Timer to know when to stop the shutter-signal.
  boolean focusing;                //True if the camera is focusing.
  boolean shuttering;              //True if the camera is taking a picture.
  
  const int adjustmentCoefficient = 0.3; //Used for adjusting and tweaking the written servo-angle based on current angle.
  
  unsigned long timer;                   //The timer measures the time between each gyroscope-reading. Used in the Kalman-filter.
  
  
  void setup() 
  {
      Wire.begin();
      Serial.begin(38400);
  
      //Initialize the MPU and attach the servos to their respective pins.
      Serial.println("Initialize MPU");
      mpu.initialize();
      Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
      servoPitch.attach(9);
      servoRoll.attach(10);
      servoYaw.attach(11);
      
      attachInterrupt(0, focus, RISING);
      attachInterrupt(1, shutter, RISING);
      pinMode(focusPin, OUTPUT);
      pinMode(shutterPin, OUTPUT);
      pinMode(focusButton, INPUT);
      pinMode(shutterButton, INPUT);
      pinMode(joyButtonPin, INPUT);
      focusing = false;
      shuttering = false;
      
      //Initialize the coordinates to fit the starting position.
      reset();
  }
  
  
  void loop() 
  {
      //Create an array to store the pitch, roll, and yaw respectively, after they've been read and computed.
      int orientation[3];             
    
      //Get the motion-data, fill the array.
      readCoordinates(orientation);
      
      //Check for user-input via joystick.
      readJoystick();

       //Re-adjust the signal to make up for physical off-set.
      //A positive offset will increase the pitch and vice versa.
      int offsetPitch = orientation[0] - zeroPitch;
      int offsetRoll = orientation[1] - zeroRoll;

      //Calculate the increased value based on current angle.
      orientation[0] = orientation[0] + (offsetPitch * adjustmentCoefficient);
      orientation[1] = orientation[1] + (offsetRoll * adjustmentCoefficient);
      
      //Keep the signals for the servos within the angles we have set.
      //We can use constrain() here instead, most likely.
      orientation[0] = constrain(orientation[0], minPitch, maxPitch);
      orientation[1] = constrain(orientation[1], minRoll, maxRoll);
      orientation[2] = constrain(orientation[2], minYaw, maxYaw);
      
      //Store the current angles for reference in the next loop.
      pitchPrevious = orientation[0];
      rollPrevious = orientation[1];
      
      //Add together the gyroscpoe data and the user-input from the joystick.
      int pitchFinal = orientation[0] + joystickVertical;
      int rollFinal = orientation[1];
      int yawFinal = yawPrevious;
      
      if (yawLocked == false)
      {
        yawPrevious = orientation[2];
      }
      
      yawFinal =+ joystickHorizontal;
      
      //Write the final angles to the servos.
      servoPitch.write(pitchFinal);
      servoRoll.write(rollFinal);
      servoYaw.write(yawFinal);
      
      //Print the values to the monitor (for testing).
      printToMonitor();
      
      //Update the timer.
      timer = micros();
      
      //If the camera is focusing, check its timer.
      //If 0.5 seconds has passed, stop the focusing.
      if (focusing == true && (timer - focusTimer) >= 500000)
      {
         digitalWrite(focusPin, LOW);
         focusing = false;
      }
      
      //If the camera is taking a picture, check the timer.
      //If 0.5 seconds has passed, set the shutter signal to low again.
      if (shuttering == true && (timer - shutterTimer) >= 500000)
      {
         digitalWrite(shutterPin, LOW);
         shuttering = false;
      }
  }
  
  
  
  /*This function reads the values from the 9-axis gyroscope, performs the necessary computations
  to apply the Kamlan-filter to the recieved values, and then maps the final values into the range
  of angles that our servos are compatible for.*/
  void readCoordinates(int orientation[])
  {
      //Some intermediate values to store the raw motion-data.
      int16_t ax, ay, az;
      int16_t gx, gy, gz;
      int16_t cx, cy, cz;
      
      int pitchGyro;                  //Stores the mapped gyro-signals for the pitch-angle.
      int pitchAccel;                 //Stores the mapped accelerator-signals for the pitch-angle.
      int rollGyro;                   //Stores the mapped gyro-signals for the roll-angle.
      int rollAccel;                  //Stores the mapped accelerator-signals for the roll-angle.
      int yawGyro;                    //Stores the mapped gyro-signals for the yaw-angle.         
      int yawAccel;                   //Stores the mapped accelerator-signals for the yaw-angle. -> REMOVE?
      int yawCompass;                 //Stores the mapped compass-signals for the yaw-angle.
    
      //Get gyroscope-, accelerometer-, and compass-data.
      //getMotion9 calls getMotion6, then reads and stores the compass-data.
      mpu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &cx, &cy, &cz);
      pitchAccel = map(ax, -17000, 17000, 0, 179);
      pitchGyro = map(gx, -33000, 33000, 0, 179);
      
      rollAccel = map(ay, -17000, 17000, 179, 0);
      rollGyro = map(gy, -33000, 33000, 179, 0);
      
      //Accellerator does not measure yaw.
      yawGyro = map(gz, -17000, 17000, 0, 179);
      //yawCompass = map(cz, -33000, 33000, 0, 179);
      
      //Apply the complimentary filter.
      //As the accellerometer does not measure the yaw/z-axis movement, we instead utilize the compass data.
      orientation[0] = (percentGyro*(pitchPrevious+(pitchGyro*(double)(micros()-timer)/1000000)))+(percentAcc*pitchAccel);
      orientation[1] = (percentGyro*(rollPrevious+(rollGyro*(double)(micros()-timer)/1000000)))+(percentAcc*rollAccel);
      orientation[2] =  (yawPrevious+(yawGyro*(double)(micros()-timer)/1000000))-1.65;
  }
  
  
  /*Simply reads the position of the joystick and alters the signal to makes sure it corresponds to the physical position.
  Also makes sure the joystick can't steer the servo past its set maximum angle.*/
  void readJoystick()
  {
      //Read the joystick-position and alter it. +/-3 is to make 0 its signal for the resting position.
      // -512 to provide equal +/- numbers. Ignore the signal if the maximum angle has been reached.
      float stickYposition = ((analogRead(joystickVerticalPin) - 512) / -50)*0.080;///5;
      float stickXposition = ((analogRead(joystickHorizontalPin) - 512) / 50)*0.04;///5; 
      
      if ((pitchPrevious + joystickVertical + stickYposition) >= maxPitch)
      {
        stickYposition = 0;
        joystickVertical -= 0.4;
      }
      else if ((pitchPrevious + joystickVertical + stickYposition) <= (minPitch))
      {
        stickYposition = 0;
        joystickVertical += 0.4;
      }
      
      if ((yawPrevious + joystickHorizontal + stickXposition) >= maxYaw)
      {
        stickXposition = 0;
        joystickHorizontal -= 0.4;
      }
      else if ((yawPrevious + joystickHorizontal + stickXposition) <= (minYaw))
      {
        stickXposition = 0;
        joystickHorizontal += 0.4;
      }
      
      //Add the input to the offset caused by the joystick.
      joystickVertical += stickYposition;
      joystickHorizontal += stickXposition;
  }
  
  //Resets the gimbals zero-angle position based on the current one.Might need to use accelerator-data here.
  //Maybe change this and use it for initializing the position on start-up as well.
  void reset()
  {
      joystickVertical = 0;
      joystickHorizontal = 0;
      servoPitch.write(zeroPitch);
      servoRoll.write(zeroRoll);
      servoYaw.write(zeroYaw);
      pitchPrevious = zeroPitch;
      rollPrevious = zeroRoll;
      yawPrevious = zeroYaw;
  }
  
  //Interupt routine for the camera-focus.
  void focus()
  {
    if (digitalRead(joyButtonPin) == HIGH)
    {
      digitalWrite(focusPin, HIGH);
      focusing = true;
      focusTimer = micros();
    }
    else
    {
      reset();
    }
  }
  
  //Interupt routine for the camera-shutter.
  void shutter()
  {
    if (digitalRead(joyButtonPin) == HIGH)
    {
      digitalWrite(shutterPin, HIGH);
      shuttering = true;
      shutterTimer = micros();
    }
    else
    {
      yawLocked = !yawLocked;
    }

  }
  
  //Prints values to the screen. For testing and monitoring.
  void printToMonitor()
  {
      Serial.print("Pitch:");
      Serial.print(pitchPrevious);
      Serial.print("\tRoll:");
      Serial.print(rollPrevious);
      Serial.print("\tYaw:");
      Serial.println(yawPrevious);
  }