#include <Arduino.h>
#include <splash.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include <NintendoExtensionCtrl.h> // https://github.com/dmadison/NintendoExtensionCtrl

#include <Wire.h>
// second I2C on PB_11 (SDA) PB_10 (SCL)
TwoWire Wire2(PB_11, PB_10);
// TwoWire Wire(PB_9, PB_8);

#define SCREEN_WIDTH 128        // OLED display width, in pixels
#define SCREEN_HEIGHT 64        // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C2 (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Connect to a Nunchuk
Nunchuk nchuk(Wire);

int cmd1;  // normalized input values. -1000 to 1000
int cmd2;


#define LED_BUILTIN PC13

// Serial1 PA_9 TX PA_10 RX

void setup() {
  Wire.setSCL(PB_8);
  Wire.setSDA(PB_9);
  Wire2.setSCL(PB_10);
  Wire2.setSDA(PB_11);

  nchuk.begin();

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3C (for the 128x64)
  display.display();

  Serial.begin(115200);
  Serial1.begin(115200);

  while (!Serial) ;
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.println("STM32 blink test");
}

void loop() {
  unsigned long timeNow = millis();
  // Blink the LED
  digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
  if (!nchuk.update()) {
    Serial.println("Controller Disconnected!");
    nchuk.reconnect();
  } else {
    cmd1 = map(nchuk.joyX(), 0, 256, 150, -150); // x - axis. Nunchuck joystick readings range 30 - 230
    cmd2 = map(nchuk.joyY(), 0, 256, -150, 150); // y - axis

    Serial.print("X : "); Serial.println(nchuk.joyX());
    Serial.print("Y : "); Serial.println(nchuk.joyY());

  }
  if ((timeNow % 2000) < 1000) {
    Serial.println(millis() / 1000);
  }

}