//--- LIBRERIAS ---//

#include <Adafruit_NeoPixel.h>
#include <Wire.h>
#include <Encoder.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050.h>
//--- DEFINICIONES ---//

#define LED_COUNT 12
#define LED_PIN 10

#define ancho1306 128 // ancho pantalla OLED
#define alto1306 64 // alto pantalla OLED

#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

Adafruit_NeoPixel strip = Adafruit_NeoPixel(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

Adafruit_SSD1306 oled1306(ancho1306, alto1306, &Wire, -1);

MPU6050 mpu6050;

//------------------------//
//----GLOBAL-VARIABLES----//
//------------------------//

int address = SCREEN_ADDRESS;

const int btn1 = 2;



//---- MPU VARIABLES ----//


const int mpuAddress = 0x68;
const float Smooth = 0.9;

int16_t acce_x, acce_y, acce_z; // variables for accelerometer raw data
int16_t gyro_x, gyro_y, gyro_z; // variables for gyro raw data
int16_t temp; // variables for temperature data

char tmp_str[7]; // temporary variable used in convert function

char* convert_int16_to_str(int16_t i) { // converts int16 to string. Moreover, resulting strings will have the same length in the debug monitor.
  sprintf(tmp_str, "%6d", i);
  return tmp_str;
}

int read = 0;
int mode = 0;
int clr = 0;
int sat = 0;
int bri = 25;

int dir = 1;
int fade = 20;

// --------- variables datos y pos. circulo -------- //
// ---- "O" al comienzo de variable = "circulo" ---- //

int OcentroX = ancho1306 / 2;
int OcentroY = ((alto1306 - 16) / 2) + 16; // top yellow part is 16 px height
int ORadio = 23;

unsigned long btnTime = 0;
long oldPosition  = 0;
long newPosition  = 0;

bool btnPress = false;
bool modeChanged;

Encoder myEnc(12, 13);


//-------------------------//


void setup() {

  Wire.begin();
  Serial.begin(115200);
  mpuReset();

  // Inicialización de la pantalla OLED
  if(!oled1306.begin(SSD1306_SWITCHCAPVCC, address)) {
   // Serial.println("NO se encuenta la Pantalla OLED");
    for(;;);
  }
  oled1306.clearDisplay();

  Wire.beginTransmission(mpuAddress); // Begins a transmission to the I2C slave (GY-521 board)
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
    
  strip.begin();
  strip.show();
  pinMode(btn1, INPUT_PULLUP);

}

void loop() {

  readEnc();
  strip.show();
  horizonDraw();
  delay(10);

  if (changeMode())
  {
    stripPulse(2, 0, 255);
  }

  for (int a = 0; a < LED_COUNT; a++) {
    setPixel(a, clr, bri);
  }

  mpu();
  oled1306.clearDisplay();

}

void mpuReset () {
 
  //extra line to ensure the MPU is reset//

  delay(1000);
  mpu6050.reset();
  Serial.println("Restarting MPU6050 after boot...");
  delay(2000);

}

void mpu (void) {

  Wire.beginTransmission(mpuAddress);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(mpuAddress, 7*2, 1); // request a total of 7*2=14 registers
  

  // "Wire.read()<<8 | Wire.read();" means two registers are read and stored in the same variable

  acce_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H) and 0x3C (ACCEL_XOUT_L)
//  Serial.print(" | aX = "); Serial.print(acce_x);

  acce_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x3D (ACCEL_YOUT_H) and 0x3E (ACCEL_YOUT_L)
  Serial.print(" | aY = "); Serial.print(acce_y);

  acce_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) and 0x40 (ACCEL_ZOUT_L)
//  Serial.print(" | aZ = "); Serial.print(acce_z);

  temp   = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H) and 0x42 (TEMP_OUT_L)
//  Serial.print(" | tmp = "); Serial.print(temp/340.00+36.53);

  gyro_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
//  Serial.print(" | gX = "); Serial.print(gyro_x);

  gyro_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
  Serial.print(" | gY = "); Serial.print(gyro_y);

  gyro_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)
//  Serial.print(" | gZ = "); Serial.print(gyro_z);

}

void horizonDraw (void) {

  float angle = (acce_y / 16000.0) * 1.570;

  Serial.print(" | angle = ");
  Serial.print(angle);

  float x0 = ORadio*(cos(angle)*0.85);
  Serial.print(" | x0 = "); Serial.print(x0);

  float y0 = (ORadio-1)*(sin(angle)*0.85);
  Serial.print(" | y0 = "); Serial.print(y0);

  float x1 = ORadio*(cos(angle)*0.85);
  Serial.print(" | x1 = "); Serial.print(x1);

  float y1 = (ORadio)*(sin(angle)*0.85);
  Serial.print(" | y1 = "); Serial.println(y1);

  oled1306.drawCircle(OcentroX, OcentroY, ORadio, WHITE);
  oled1306.drawLine(OcentroX-x0, OcentroY+y0, OcentroX+x1, OcentroY-y1, WHITE);

  oled1306.display();
}

void setPixel(int pixelz, int colorz, long unsigned fadez) {
  fadez *= fadez;
  fadez /= 255;
  strip.setPixelColor(pixelz, strip.ColorHSV(colorz * 64, sat, fadez));
}

void readEnc(void) {

  long newPosition = myEnc.read();

  if(newPosition<0)
  {
    newPosition=0;
    myEnc.write(0);
  }
  if(newPosition>255)
  {
    newPosition=255;
    myEnc.write(255);
  }

  if (newPosition != oldPosition) {
    oldPosition = newPosition;
  }
    
  
  if (!btnPress) 
    switch(mode)
    {
      case 0:
        bri = newPosition;
        break;
      case 1:
        sat = newPosition;
        break;
      case 2:
        clr = map(newPosition*2,0,255,0,1024/2);
        break;
    }
    
}

bool changeMode(void) {

  if (digitalRead(btn1) == LOW) {
    if (btnTime == 0) {
      btnPress = true;
      btnTime = millis();
    } else if (millis() - btnTime >= 300) {
      mode = (mode + 1) % 3;
      btnTime = 0;
      delay(300);
      return true;
    }
  } else {
    btnTime = 0;
    btnPress = false;
  }

  return false;
}

void stripPulse(uint8_t times, uint8_t minBri, uint8_t maxBri) {

  uint8_t i,k,l;
  uint16_t j;

  for (i=0; i < times; i++) {
    for( j=minBri; j<maxBri; j+=5 )
    {
      for (int k = 0; k < LED_COUNT; k++)
        setPixel(k,clr,j);
      strip.show();
      delay(5);
    }

    if(i==times-1)
      minBri=bri;

    for( j=maxBri; j>minBri; j-=5 )
    {
      for (int k = 0; k < LED_COUNT; k++)
        setPixel(k,clr,j);
      strip.show();
      delay(5);
    }
  }
  delay(300);
}