#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <NeoPixelBus.h>
#include <WiFi.h>
#include <PubSubClient.h>

#define colorSaturation 5
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 27  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 2 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

enum mode { DEFAULT_MODE, PLAY_MODE, IMAGE_MODE };

const uint16_t PixelCount = 64; // this example assumes 4 pixels, making it smaller will cause a failure
const uint8_t PixelPin = 26;  // make sure to set this to the correct pin, ignored for Esp8266
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[128]; // FIFO storage buffer
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
Quaternion q;
VectorFloat gravity;    // [x, y, z]            gravity vector
MPU6050 mpu;

//const uint16_t PixelCount = 64; // this example assumes 4 pixels, making it smaller will cause a failure
//bool blinkState = false;
char* ssid = "504";
char* password =  "20142020";
const char* mqttServer = "m15.cloudmqtt.com";
const int mqttPort = 15861;
const char* mqttUser = "llaoendv";
const char* mqttPassword = "jULqWpRt3vRx";


int n = 2;

NeoPixelBus<NeoGrbFeature, Neo800KbpsMethod> strip(PixelCount, PixelPin);
WiFiClient espClient;
PubSubClient client(espClient);

int foo;

void dmpDataReady() {
    mpuInterrupt = true;
}



void show_array(byte* Data) {
  RgbColor Colors[8][8];
  int k = 0;

  if (Data[0] == 0) {
    for (int i = 0; i < 8; i++) {
      for (int j = 0; j < 8; j++) {
        Colors[i][j] = RgbColor(Data[k + 1], Data[k + 2], Data[k + 3]);
        strip.SetPixelColor(i * 8 + j, Colors[i][j]);
        k += 3;
      }
    }
  } else {}
  strip.Show();
}

void callback(char* topic, byte* payload, unsigned int length) {
  char message[256] = "";
  byte Data[256];

  for (int i = 0; i < 256; i++) {
    if (i < length) {
      Data[i] = payload[i];
      message[i] = (char)payload[i];
    } else {
      Data[i] = 0;
    }
  }
  show_array(Data);
  Serial.println(message);
  Serial.println("-----------------------");
  n = 2;
}

void setSensorConnect() {
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);

  if (devStatus == 0) {
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}


void setWifiConnect() {
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.println("Connecting to WiFi..");
    delay(1000);
  }
  Serial.println("Connected to the WiFi network");
}


void setMqttConnect() {
  client.setServer(mqttServer, mqttPort);
  client.setCallback(callback);

  while (!client.connected()) {
    Serial.println("Connecting to MQTT...");
    if (client.connect("ESP32Client", mqttUser, mqttPassword )) {
      Serial.println("connected");
    } else {
      Serial.print("failed with state ");
      Serial.print(client.state());
      delay(2000);
    }
  }
  client.subscribe("esp/test");
  client.publish("esp/test_hi", "Hello from ESP32");
}

void setup() {
    Serial.begin(115200);
    while (!Serial);

    //setSensorConnect();
    setWifiConnect();
    setMqttConnect();

    strip.Begin();

    for (int i = 0; i < PixelCount; i++) {
      strip.SetPixelColor(i, RgbColor(0, 0, 0));
    }


    switch (n) {
      case DEFAULT_MODE:
        break;

      case PLAY_MODE:
        foo = generateFood();
        strip.SetPixelColor(foo, RgbColor(50, 0, 0));
        break;

      case IMAGE_MODE:
        break;
    }

    strip.Show();
    pinMode(LED_PIN, OUTPUT);
}


int generateFood() {
  return random(1, 64);
}


void playLoop() {
  if (!dmpReady) return;

  while (!mpuInterrupt && fifoCount < packetSize) {
    if (mpuInterrupt && fifoCount < packetSize) {
      fifoCount = mpu.getFIFOCount();
    }
  }

  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();

  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));
  } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
      while (fifoCount < packetSize) {
        fifoCount = mpu.getFIFOCount();
      }

      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;

      #ifdef OUTPUT_READABLE_YAWPITCHROLL

      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      #endif


      for (int i = 0; i < PixelCount; i++) {
        if (i != foo)
          strip.SetPixelColor(i, RgbColor(0, 0, 0));
      }

      int pxl1 = (ypr[1] * 180/M_PI);
      pxl1 = (pxl1 > 0) ? (pxl1 - 180) : (pxl1 + 180);
      pxl1 /= 10;
      pxl1 *= -8;
      pxl1 += 32;

      int pxl2 = (ypr[2] * 180/M_PI);
      pxl2 = (pxl2 > 0) ? (pxl2 - 180) : (pxl2 + 180);
      pxl2 /= 10;
      pxl2 += 4;

      if ((pxl2 >= 0)
          && (pxl2 <= 7)
          && (pxl1 + pxl2 >= 0)
          && (pxl1 + pxl2 <= 63)) {

        strip.SetPixelColor(pxl1 + pxl2, RgbColor(0, colorSaturation, 0));
        if (pxl1 + pxl2 == foo) {
          foo = generateFood();
          strip.SetPixelColor(foo, RgbColor(50, 0, 0));
        }
      } else {
        for (int i = 0; i < PixelCount; i++) {
          strip.SetPixelColor(i, RgbColor(20, 0, 0));
        }
      }
    }

    strip.Show();
}

void loop() {
    switch (n) {
    case DEFAULT_MODE:
      break;

    case PLAY_MODE:
      playLoop();
      break;

    case IMAGE_MODE:
      client.loop();
      break;
  }
}