#include <QMC5883L.h>
#include <Adafruit_SH1106.h>
#include <Adafruit_GFX.h>
#include <AccelStepper.h>
#include <Wire.h>
#include <SPI.h>

#define OLED_RESET     4
#define SCREEN_ADDRESS 0x3C
#define STEPS 200 //because of gear ratio one full rotation of the camera is 400 steps, therefore, we only need to carry out 200 steps per 180 degrees or one night
#define MOTOR_BUTTON 12
#define END_STOP 25
#define INTERVAL 13500 //how often one step is taken
#define DEBOUNCE 50;
Adafruit_SH1106 display(OLED_RESET);
QMC5883LCompass compass;

AccelStepper stepper(AccelStepper::DRIVER, 32, 33); //32 STEP, 33, DIR
long lastStep = 0;
bool motorBinary = false;
bool prevButtonState = LOW;
unsigned long milSec = 0;
unsigned long prevSec = 0;
unsigned long lastButton = 0;
int heading; //direction heading
char dir[3]; //direction bytes
void setup() {
  Serial.begin(9600);
  Wire.begin(21,22);
  pinMode(MOTOR_BUTTON, INPUT_PULLDOWN);  
  pinMode(END_STOP, INPUT_PULLDOWN);
  compass.init();
  display.begin(SH1106_SWITCHCAPVCC, SCREEN_ADDRESS);
  //magneto sensor
  compass.setMode(0x01, 0x00, 0x00, 0x80);
  compass.setCalibration(-1537, 1266, -1961, 958, -1342, 1492); //CALIBRATION HAS TO BE CALCULATED
  compass.setMagneticDeclination(12, 43);
  //Magnetic Declination: +12° 43'
  //stepper
  stepper.move(10); //move foward a bit
  stepper.run();
  if(digitalRead(END_STOP) == LOW){ // if switch isnt pressed
    while(digitalRead(END_STOP) == LOW){ //digital switch = low
    stepper.setSpeed(1);
    stepper.runSpeed();
    }
  }else{ //if switch is pressed, motor is at home 
    stepper.setCurrentPosition(0);
  }
}
void loop() { //loop will be the compass + standby
  long currentMillis = millis();
  milSec = millis() - prevSec; //for total time
  heading = compass.getAzimuth();
  displayAngle();

  bool butState = digitalRead(MOTOR_BUTTON); // toggle switch using one button 

  if (butState != prevButtonState){
    delay(50);
    if (butState == LOW){
      motorBinary = !motorBinary;
      if(motorBinary){
        prevSec = millis();
      }
    }
  }
  prevButtonState = butState;

  if(motorBinary && milSec <= 3600000 && stepper.currentPosition() <= 3200){ //3600000 is max limit, 10 hours, 3200 is half the full rotation
    if(currentMillis - lastStep >= INTERVAL){
      stepper.step(1);
      lastStep = currentMillis;
    }
  }else if (motorBinary == false){
    stepper.setSpeed(0);
  }
}

void displayAngle(){
  heading = compass.getAzimuth();
  getDirection(dir, heading);
  String direction = String(dir[0]) + String(dir[1]) + String(dir[2]);
  //show angle
  display.setTextColor(WHITE);
  display.setCursor(45,5); //prints angle 
  display.println(heading);
  display.setCursor(44,38); //prints directiion: N E W S, etc
  display.println(direction);
}