#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h>
//#include <Thread.h>

Adafruit_MPU6050 mpu;

#define PIN_RED    23 // GPIO23
#define PIN_GREEN  22 // GPIO22
#define PIN_BLUE   21 // GPIO21

#define B_BUTTON 25

#define SCL 18
#define SDA 19

int mpuAddress = 0x68;

const int i2c_frequency = 400000;

int color = PIN_RED;
void control_led_color (void *pvParameters) {
Serial.println("read button");
int phase = 0;
for (;;) {
int green = abs(sin(phase) * 0.1) * 2550;
//Serial.print("brithness: ");
//Serial.println(green);
analogWrite(color, green);
delay(200);
for (int i = 0; i < green / 4; i++) {
  Serial.print("=");
}
Serial.println("");
phase++;
}
}

int prev_color = PIN_RED;
void read_button (void *pvParameters) {
  for (;;) {
    if (digitalRead(B_BUTTON) == HIGH) {
      if (prev_color != PIN_GREEN) {
      color = PIN_GREEN;
      analogWrite(PIN_RED, 0);
      prev_color = color;
      }
    }
    else {
      if (prev_color != PIN_RED) {
      color = PIN_RED;
      analogWrite(PIN_GREEN, 0);
      prev_color = color;
      }
    }
    delay(1);
  }
}

TaskHandle_t led_control_task, button_read_task;
void setup() {
  Serial.begin(115200);
  pinMode(PIN_RED,   OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE,  OUTPUT);

  pinMode(B_BUTTON, INPUT);
  Serial.print("Brightness button state: ");
  Serial.println(digitalRead(B_BUTTON) == HIGH);

//  pinMode(19, INPUT);
//  pinMode(18, INPUT);

xTaskCreatePinnedToCore(
  read_button,
  "Read Button",
  10000,
  NULL,
  1,
  &button_read_task,
  0
);

xTaskCreatePinnedToCore(
  control_led_color,
  "Control LED Color",
  1000,
  NULL,
  1,
  &led_control_task,
  0
);

Wire.begin(SDA, SCL, 400000);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}

/*
int loop_counter = 0;
void loop() {
    int color_pin = PIN_RED;
    int pressedBrightness = digitalRead(B_BUTTON);
    if (pressedBrightness == HIGH) { 
      color_pin = PIN_GREEN;
//      Serial.print("Brightness pressed: ");
  //    Serial.println(loop_counter);
      loop_counter++;
  }
  for (int i = 0; i < 255; i++) {
    analogWrite(color_pin, i);
    delay(3);
  }
  delay(500);

  for (int i = 255; i > 0; i--) {
    analogWrite(color_pin, i);
    delay(3);
  }
  delay(500);
}
*/
/*
int forward = 1;
void loop () {
  if (forward) {
    for (int i = 0; i < 255; i++) {
      colorize_led(i);
      delay(2);
    }
  }
  else {
    for (int i = 255; i >= 0; i--) {
      colorize_led(i);
    }
  }
  forward = !forward;
//  Serial.print("forward: ");
//  Serial.println(forward);
  delay(400);
}

void b_button_pressed () {
  analogWrite(PIN_RED, 0);
  Serial.println("B pressed");
}

void b_button_released () {
  analogWrite(PIN_GREEN, 0);
  Serial.println("B released");
}

int is_b_pressed = 0;
int is_b_button_pressed () {
  int is_pressed = digitalRead(B_BUTTON) == HIGH;
  if (!is_pressed && is_b_pressed) {
    b_button_released();
  }
  else if (is_pressed && !is_b_pressed) {
    b_button_pressed();
  }
  is_b_pressed = is_pressed;
  return is_pressed;
}

int get_led_pin () {
      int led_pin = PIN_RED;
      if (is_b_button_pressed()) {
        led_pin = PIN_GREEN;
      }
      return led_pin;
}

int prev_led = 0;
void colorize_led (int color_value) {
  int led_pin = get_led_pin();
/*  if (led_pin != prev_led) {
    analogWrite(prev_led, 0);
  }* /
  analogWrite(led_pin, color_value);
}
*/

int gyroX, gyroY, gyroZ, accX, accY, accZ, temp;
void loop () {
  Wire.beginTransmission( mpuAddress);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start

  // The MPU-6050 has the values as signed 16-bit integers.
  // There are 7 values in 14 registers.
  int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

  Wire.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)

Serial.println(GyX);

  delay(20);
}