#include "engine.h"
#include "joystick.h"
#include "simplejoystick.h"
#include "display.h"
#include "flightdata.h"

SimpleJoystick joystick1(A0, A1, 22, 26);
SimpleJoystick joystick2(A2, A3, 23, 27);
Display centerDisplay(25, 24);
Display leftDisplay(29, 28);
Display rightDisplay(31, 30);
Gauge gaugeAngle = centerDisplay.createGauge(0, 0, "Angle");
Gauge gaugeSpeed = centerDisplay.createGauge(0, 1, "Speed");
Gauge gaugeHeight = centerDisplay.createGauge(1, 0, "Height");
Gauge gaugeRotation = centerDisplay.createGauge(1, 1, "Rotation");
FlightData (A4);

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

  centerDisplay.setup();
  leftDisplay.setup();
  rightDisplay.setup();
  gaugeAngle.setup();
  gaugeSpeed.setup();
  gaugeHeight.setup();
  gaugeRotation.setup();

  centerDisplay.clearScreen();
  gaugeAngle.draw();
  gaugeSpeed.draw();
  gaugeHeight.draw();
  gaugeRotation.draw();
}

void loop() {
  JoystickOutput output1 = joystick1.read();
  JoystickOutput output2 = joystick2.read();
  gaugeAngle.setAngle(output2.angle);
  gaugeSpeed.setAngle(output2.angle);
  gaugeHeight.setAngle(output1.angle);
  gaugeRotation.setAngle(output1.angle);
}