#include <Arduino.h>
#include "splash-h.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);
// Connect to a Nunchuk
Nunchuk nchuk(Wire2);

// ########################## DEFINES ##########################
#define LED_BUILTIN PC13
#define TIME_SEND 20            // [ms] poolling time interval
#define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define START_FRAME 0xAAAA      // [-] Start frme definition for reliable serial communication
#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);

// Global variables
uint8_t idx = 0;        // Index for new data pointer
uint16_t bufStartFrame; // Buffer Start Frame
byte *p;                // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;

typedef struct
{
  uint16_t start;
  int16_t steer;
  int16_t speed;
  uint16_t checksum;
} SerialCommand;
SerialCommand Command;

typedef struct
{
  uint16_t start;
  int16_t cmd1;
  int16_t cmd2;
  int16_t speedR;
  int16_t speedL;
  int16_t speedR_meas;
  int16_t speedL_meas;
  int16_t batVoltage;
  int16_t boardTemp;
  int16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;

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

// Serial1 PA_9 TX PA_10 RX
// ########################## SETUP ##########################
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.clearDisplay();
  display.drawBitmap((SCREEN_WIDTH - my_splash_width) / 2, (SCREEN_HEIGHT - my_splash_height) / 2,
                     my_splash_data, my_splash_width, my_splash_height, 1);
  
  /*
  display.setTextSize(1); // Draw 1X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(int((display.width() - (strlen("g")*6))), 50);
  display.println("g");
  */
  display.display();

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

  while (!Serial)
    ;
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.println("STM32 hoverboard control");
  display.clearDisplay();
  display.setTextSize(1); // Draw 1X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(int((display.width() -
                         (strlen("Hoverboard Control") * 6)) /
                        2),
                    0);
  display.println("Hoverboard Control");
  display.setCursor(int((display.width() - (strlen("v STM32 0.1") * 6)) / 2), 8);
  display.println("v STM32 0.1");
}
// ########################## SEND ##########################
void Send(int16_t uSteer, int16_t uSpeed)
{
  // Create command
  Command.start = (uint16_t)START_FRAME;
  Command.steer = (int16_t)uSteer;
  Command.speed = (int16_t)uSpeed;
  Command.checksum =
      (uint16_t)(Command.start ^ Command.steer ^ Command.speed);

  // Write to Serial
  Serial1.write((uint8_t *)&Command, sizeof(Command));
}

// ########################## Nunchuk_control ##########################

void Nunchuk_control()
{
  int old_cursorX, old_cursorY;

  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.print(nchuk.joyX());
    Serial.print(" Y : ");
    Serial.println(nchuk.joyY());
*/
    Send(cmd1, cmd2);
  }
  display.setCursor(0, 40);
  display.setTextSize(1); // Draw 1X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.print("X : ");
  old_cursorX = display.getCursorX();
  old_cursorY = display.getCursorY();
  display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
  display.setCursor(old_cursorX, old_cursorY);
  display.print(cmd1);
  display.setCursor(0, 50);
  display.setTextSize(1); // Draw 1X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.print("y : ");
  old_cursorX = display.getCursorX();
  old_cursorY = display.getCursorY();
  display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
  display.setCursor(old_cursorX, old_cursorY);
  display.print(cmd2);
  display.display();
}

// ########################## LOOP ##########################

unsigned long iTimeSend = 0;

void loop()
{
  unsigned long timeNow = millis();
  // Blink the LED
  digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
  // if (iTimeSend > timeNow)  return;
  iTimeSend = timeNow + TIME_SEND;
  Nunchuk_control();
  Serial.println(timeNow);
}