#include <Arduino.h>
#include <avr/wdt.h>

#include "spm4648.h"
#include "MPU6050.h"

#include "globals.h"
//#include "driveCircle.h"
//#include "driveRegler.h"
#include "drivefusion.h"


bool lastSwitch = true;
uint32_t mainLoopDelay = 16000;
uint32_t mainLoopStart = 0;

void driveRemote() {
  if(spm.hasData()) {
    setMotor(spm.data[2]*0.3);
    setServo(spm.data[1]);
    Serial.print("Manual: \t");
    Serial.print(gasVal);
    Serial.print(" \t");
    Serial.print(servoVal);
    Serial.print(" \t");
    Serial.print(spm.data[2]*0.2);
    Serial.print("\t");
    Serial.println(spm.data[1]);
  }
}


void setup() {

  Serial.begin(115200);
  Serial1.begin(115200);
  Serial2.begin(38400);
  Serial3.begin(9600);

  Serial.println("boot");
  mpu.setUpMPU();
  mpu.callibrateGyroValues();
  initServos();
  Serial.println("setup finished");

  wdt_enable(WDTO_250MS);
}

void loop() {
  wdt_reset();
  spm.read(Serial1);

  
  mpu.readAndProcessGyroData();
  
  mpu.readAndProcessAccelData();
  mpu.printData();
  
  if(spm.isConnected()) {
    bool sw = spm.data[4] > 0;
    if(sw == false && lastSwitch == true) {
      initAuto();
    }
    if(sw) {
      msAuto = millis();
      driveRemote();
    } else {
      driveAuto();
    }
    lastSwitch = sw;
    
  } else {
    //safe mode
    Serial.println(F("No connection!"));
    setMotor(0);
    setServo(0);
  }
  
  mainLoopDelay = micros() - mainLoopStart;
  
  if(mainLoopDelay < 10000)
    delayMicroseconds(10000 - mainLoopDelay); //max. 16ms

  mainLoopDelay = micros() - mainLoopStart;
  //Serial.println(mainLoopDelay/1000.0);

  mainLoopStart = micros();
}