#include "Joysticks.h"
#include "Nrf.h"
#include "SerialPasstrough.h"
#include <TinyMPU6050.h>

MPU6050 mpu (Wire);

dataStruct2 data;
joyStickdataStruct joystickData;

void setup() {
  Serial.begin(115200);

  // Initialization
  mpu.Initialize();

  //  // Calibration
  //  Serial.println("=====================================");
  //  Serial.println("Starting calibration...");
  //  mpu.Calibrate();
  //  Serial.println("Calibration complete!");
  //  Serial.println("Offsets:");
  //  Serial.print("AccX Offset = ");
  //  Serial.println(mpu.GetAccXOffset());
  //  Serial.print("AccY Offset = ");
  //  Serial.println(mpu.GetAccYOffset());
  //  Serial.print("AccZ Offset = ");
  //  Serial.println(mpu.GetAccZOffset());
  //  Serial.print("GyroX Offset = ");
  //  Serial.println(mpu.GetGyroXOffset());
  //  Serial.print("GyroY Offset = ");
  //  Serial.println(mpu.GetGyroYOffset());
  //  Serial.print("GyroZ Offset = ");
  //  Serial.println(mpu.GetGyroZOffset());

  JoystickInit();

  NrfInit();
}

void loop() {
  //  mpu.Execute();
  //  Serial.print("AngX = ");
  //  Serial.print(mpu.GetAngX());
  //  Serial.print("  /  AngY = ");
  //  Serial.print(mpu.GetAngY());
  //  Serial.print("  /  AngZ = ");
  //  Serial.println(mpu.GetAngZ());

  joystickData = JoystickGet(true);

  data = GetSerialData(data, false);

  //  data.servoPositions[0] = joystickData.value[0];
  data.remotePasstroughControl = data.remotePasstroughControl + 1;

  data = NrfSendData(data, true);

  delay(20);
}