#include <Arduino.h>
#include "splash-h.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_NeoPixel.h>

#define PIN PB5   // Pin where NeoPixels are connected
#define NB_LED 12 // number of LEDs

// Declare our NeoPixel strip object:
Adafruit_NeoPixel strip(NB_LED, PIN, NEO_GRB + NEO_KHZ800);
// Argument 1 = Number of pixels in NeoPixel strip
// Argument 2 = Arduino pin number (most are valid)
// Argument 3 = Pixel type flags, add together as needed:
//   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
//   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
//   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
//   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
//   NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)

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

#include <Wire.h>
// second I2C on PB11 (SDA) PB10 (SCL)
TwoWire Wire2(PB11, PB10);
// Connect to a Nunchuk
Nunchuk nchuk(Wire2);

// ########################## DEFINES ##########################
#define LED_BUILTIN PC13
#define TIME_SEND 100           // [ms] 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;

// ########################## colorWipe ##########################
// Fill strip pixels one after another with a color. Strip is NOT cleared
// first; anything there will be covered pixel by pixel. Pass in color
// (as a single 'packed' 32-bit value, which you can get by calling
// strip.Color(red, green, blue) as shown in the loop() function above),
// and a delay time (in milliseconds) between pixels.
void colorWipe(uint32_t color, int wait)
{
  for (int i = 0; i < strip.numPixels(); i++)
  {                                // For each pixel in strip...
    strip.setPixelColor(i, color); //  Set pixel's color (in RAM)
    strip.show();                  //  Update strip to match
    delay(wait);                   //  Pause for a moment
  }
}

// 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.display();

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

  strip.begin(); // INITIALIZE NeoPixel strip object
  strip.clear();
  strip.show();            // Turn OFF all pixels ASAP
  strip.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)

  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");

  // Fill along the length of the strip in various colors...

  colorWipe(strip.Color(255, 0, 0), 20); // Red
  colorWipe(strip.Color(0, 255, 0), 20); // Green
  colorWipe(strip.Color(0, 0, 255), 20); // Blue
  colorWipe(strip.Color(0, 0, 0), 20);   // Black
}
// ########################## 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));
}
// ########################## RECEIVE ##########################
void Receive()

{
  int old_cursorX, old_cursorY;
  // Check for new data availability in the Serial buffer
  if (Serial1.available())
  {
    incomingByte = Serial1.read();                                      // Read the incoming byte
    bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame
  }
  else
  {
    return;
  }

// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
  Serial.print(incomingByte);
  return;
#endif

  // Copy received data
  if (bufStartFrame == START_FRAME)
  { // Initialize if new data is detected
    p = (byte *)&NewFeedback;
    *p++ = incomingBytePrev;
    *p++ = incomingByte;
    idx = 2;
  }
  else if (idx >= 2 && idx < sizeof(SerialFeedback))
  { // Save the new received data
    *p++ = incomingByte;
    idx++;
  }
  // Check if we reached the end of the package
  if (idx == sizeof(SerialFeedback))
  {
    uint16_t checksum;
    checksum =
        (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);

    // Check validity of the new data
    if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum)
    {
      // Copy the new data
      memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));

      // Print data to CDC Serial if available
      if (Serial)
      {
        Serial.print("1: ");
        Serial.print(Feedback.cmd1);
        Serial.print(" 2: ");
        Serial.print(Feedback.cmd2);
        Serial.print(" 3: ");
        Serial.print(Feedback.speedR);
        Serial.print(" 4: ");
        Serial.print(Feedback.speedL);
        Serial.print(" 5: ");
        Serial.print(Feedback.speedR_meas);
        Serial.print(" 6: ");
        Serial.print(Feedback.speedL_meas);
        Serial.print(" 7: ");
        Serial.print(Feedback.batVoltage);
        Serial.print(" 8: ");
        Serial.println(Feedback.boardTemp);
      }
      display.setCursor(0, 30);
      display.setTextSize(1); // Draw 1X-scale text
      display.setTextColor(SSD1306_WHITE);
      display.print("V : ");
      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(Feedback.batVoltage);
      display.display();
    }
    else
    {
      if (Serial)
        Serial.println("Non-valid data skipped");
    }

    idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
  }
  // Update previous states
  incomingBytePrev = incomingByte;
}

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

void Nunchuk_control()
{
  int old_cursorX, old_cursorY;

  if (!nchuk.update())
  {
    nchuk.reconnect();
  }
  else
  {

    cmd1 = map(nchuk.joyX(), 0, 256, 150, -150); // x - axis. Nunchuck joystick readings range 150 -150
    cmd2 = map(nchuk.joyY(), 0, 256, -150, 150); // y - axis

    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;
int idx_led = 0;

void loop()
{
  unsigned long timeNow = millis();

  // Blink the LED
  digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
  Receive();
  Nunchuk_control();

  if (iTimeSend > timeNow)
    return;
  if (cmd2 == 0)
    return;
  iTimeSend = timeNow + ((200 - abs(cmd2)) * 2);  // TIME_SEND
  strip.clear();
  strip.setPixelColor(idx_led, strip.Color(128, 0, 0));
  if (cmd2 > 0){
      if (idx_led < strip.numPixels() -1 )
      {
        idx_led++;
      }
      else
      {
        idx_led = 0;
      }
  }
  else
  {
     if (idx_led > 0 )
      {
        idx_led--;
      }
      else
      {
        idx_led = strip.numPixels() -1 ;
      }
  }
      strip.show();
}