#include "Joysticks.h"
#include "Nrf.h"
#include "SerialPasstrough.h"
#include <Wire.h>
#include <MPU6050_tockn.h>
#include "Nextion.h"

NexPage page0 = NexPage(4, 0, "uploadscreen");

NexText t0 = NexText(1, 6, "t1");
NexText t1 = NexText(3, 22, "t1");

NexButton b2 = NexButton(1, 5, "b2");
NexSlider sliders[18] = {NexSlider(3, 1, "h0"),
                         NexSlider(3, 2, "h1"),
                         NexSlider(3, 3, "h2"),
                         NexSlider(3, 4, "h3"),
                         NexSlider(3, 5, "h4"),
                         NexSlider(3, 6, "h5"),
                         NexSlider(3, 7, "h6"),
                         NexSlider(3, 8, "h7"),
                         NexSlider(3, 9, "h8"),
                         NexSlider(3, 10, "h9"),
                         NexSlider(3, 11, "h10"),
                         NexSlider(3, 12, "h11"),
                         NexSlider(3, 13, "h12"),
                         NexSlider(3, 14, "h13"),
                         NexSlider(3, 15, "h14"),
                         NexSlider(3, 16, "h15"),
                         NexSlider(3, 17, "h16"),
                         NexSlider(3, 18, "h17")
                        };

char buffer[100] = {0};

NexTouch *nex_listen_list[] =
{
  &b2,
  &page0,
  &t0,
  &t1,
  NULL
};

void b2PopCallback(void *ptr)
{
  passtrough();
}

MPU6050 mpu (Wire);

dataStruct2 data;
joyStickdataStruct joystickData;

void setup() {
//  Serial.begin(250000, SERIAL_8E1); // ARDUINO
  Serial.begin(250000); // TEENSY
  //  Serial.begin(115200);
  Serial1.begin(115200);
  // passtrough();

  nexInit();
  b2.attachPush(b2PopCallback, &b2);
  b2.attachPop(b2PopCallback, &b2);
  //  for (int i = 0; i < 18; i++)
  //    sliders[i] = NexSlider(3, i++, "h" + i);
  //  while (true)
  //  {
  //    nexLoop(nex_listen_list);
  //    sliders[0].setValue(30);
  //  }

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

  JoystickInit();

  NrfInit();
}

void passtrough()
{
  delay(100);
  for (int i = 0; i < 10; i++)
  {
    //    nexLoop(nex_listen_list);
    //    delay(10);
    page0.show();
    delay(10);
  }

  Serial.begin(115200);
  Serial1.begin(115200);
  do {
    if (Serial.available()) {      // If anything comes in Serial (USB),
      Serial1.write(Serial.read());   // read it and send it out Serial1 (pins 0 & 1)
    }

    if (Serial1.available()) {     // If anything comes in Serial1 (pins 0 & 1)
      Serial.write(Serial1.read());   // read it and send it out Serial (USB)
    }
  } while (true);
}

void loop()
{
  nexLoop(nex_listen_list);
  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());

  joystickData = JoystickGet(true);

  // test
  if (false)
  {
    data.remotePasstroughControl = true;
    data.servoPositions[1] = (joystickData.value[2] * 0.78f);
    data.servoPositions[4] = (joystickData.value[2] * 0.78f);
    data.servoPositions[7] = (joystickData.value[2] * 0.78f);

    data.servoPositions[10] = 180 - (joystickData.value[2] * 0.78f);
    data.servoPositions[13] = 180 - (joystickData.value[2] * 0.78f);
    data.servoPositions[16] = 180 - (joystickData.value[2] * 0.78f);
  }

  GetSerialData(data, false);
  //  data = GetSerialData(data, false);

  //  data.STOP = true;

  for (int i = 0; i < 18; i++)
    sliders[i].setValue(data.servoPositions[i]);

  bool ok = NrfSendData(data, false);
  //  data = NrfSendData(data, false);

  if (ok)
    t0.setText("connected");
  else
    t0.setText("not connected");
  if (ok)
    t1.setText("connected");
  else
    t1.setText("not connected");

  //test
  //  data = NrfGetData(data, true);

  delay(20);
}