#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 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;
// ########################## 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.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;

void loop()
{
  unsigned long timeNow = millis();
  // Blink the LED
  digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
  Receive();
  Nunchuk_control();
  Serial.println(timeNow);
}