#include "Nrf.h"
#include <Wire.h>
#include <MPU6050_tockn.h>
#include "PWMServo.h"
#include "config.h"

PWMServo servos[18];

MPU6050 mpu (Wire);

dataStruct2 data;

void setup()
{
  Serial.begin(115200);
  Serial1.begin(115200);
  //  while (!Serial) ; // wait for arduino serial monitor

  for (int i = 0; i < 18; i++)
    servos[i].attach(pins[i], servoMin[i] + servoCallibration[i],
                     servoMax[i] + servoCallibration[i]); // 900-2100 for high torque servos
//  while (true)
//  {
    for (int i = 0; i < 18; i++)
      servos[i].write(90 + servoOffset[i]);
//  }


  Serial.println("Starting mpu6050");

//  //   Initialization
//  Wire.begin();
//  mpu.begin();
//
//  mpu.calcGyroOffsets(true, 300, 1000);
//
//  // Calibration
//  Serial.println("=====================================");
//  Serial.println("Starting calibration...");
//  mpu.calcGyroOffsets();
//  Serial.println("Calibration complete!");

  Serial.println("Starting nrf");
  NrfInit();
}

void STOP()
{
  for (int i = 0; i < 18; i++)
    pinMode(pins[i], INPUT);
  while (true)
  {}
}

int i = 0;

void loop()
{
  network.update();

  if (false)
  {
    mpu.update();
    Serial.print("AngX = ");
    Serial.print(mpu.getAngleX());
    Serial.print("  /  AngY = ");
    Serial.print(mpu.getAngleY());
    Serial.print("  /  AngZ = ");
    Serial.println(mpu.getAngleZ());
  }

  //    Serial.print("  /  AngX = ");
  //    Serial.print(mpu.getGyroAngleX());
  //    Serial.print("  /  AngY = ");
  //    Serial.print(mpu.getGyroAngleY());
  //    Serial.print("  /  AngZ = ");
  //    Serial.println(mpu.getGyroAngleZ());

  data = NrfGetData(data, true);

//  if (data.STOP)
//    STOP();

  // test
  //  data.servoPositions[0] = data.servoPositions[0] + 0.1f;
  //  data = NrfSendData(data, false);

  if (data.remotePasstroughControl == true)
    for (int i = 0; i < 18; i++)
      servos[i].write(data.servoPositions[i] + servoOffset[i]);

  delay(20);
}