#include <ESP32Servo.h>
//#include "EasyBuzzer.h"
#include <NewPing.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>

#define SERVO_PIN 27
#define POT_PIN   36
#define BUTTON_1_PIN 13
#define BUTTON_2_PIN 15

#define BUTTON_3_PIN 33
#define BUTTON_4_PIN 32
#define BUTTON_5_PIN 35

#define BUZZER_PIN 4 //cambiar

#define MODE_CC 0
#define MODE_CA 1
#define SEG_1_PIN 5
#define SEG_2_PIN 18
#define SEG_3_PIN 19
#define SEG_4_PIN 26
#define SEG_5_PIN 25
#define SEG_6_PIN 23

#define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     14  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200

int angulo = 0;
bool buzzer_flag = false;

#define CC 0
#define CA 1

class LedRGB 
{
  public:
   LedRGB(int pinR, int pinG, int pinB, int modo);
   ~LedRGB();
   void setColor(int CR, int CG, int CB);
   void setColor(String color);
   
   //void defaultColor(String color);
   
  private:  
   void setColorCA(int crA, int cgA, int cbA);
   void setColorCC(int crC, int cgC, int cbC);
   int _pinR;
   int _pinG;
   int _pinB;
   int _modo;

};

LedRGB::LedRGB(int pinR, int pinG, int pinB, int mode)
{
	_pinR = pinR;
	_pinG = pinG;
	_pinB = pinB;
	pinMode(_pinR, OUTPUT);
	pinMode(_pinG, OUTPUT);
	pinMode(_pinB, OUTPUT);
	_modo = mode;
}

LedRGB::~LedRGB()
{

}

void LedRGB::setColor(int CR, int CG, int CB){
	if(_modo == 0)
  {
		setColorCC(CR, CG, CB);
	}else if(_modo == 1)
  {
		setColorCA(CR, CG, CB);
  }
}

void LedRGB::setColor(String color)
{
  if(color == "white")
  {
		setColor(255, 255, 255);
	}
  else if(color == "red")
  {
		setColor(255, 0, 0);
	}
  else if(color == "green")
  {
		setColor(0, 255, 0);
	}
  else if(color == "blue")
  {
		setColor(0, 0, 255);
	}
  else if(color == "yellow")
  {
		setColor(255, 255, 0);
	}
  else if(color == "orange")
  {
		setColor(255, 150, 0);
	}
  else if(color == "violet")
  {
		setColor(150, 0, 255);
	}
  else if(color == "turquoise")
  {
		setColor(0, 255, 220);
	}
  else if(color == "grey")
  {
		setColor(70, 70, 70);
	}
  else if(color == "maroon")
  {
		setColor(176, 54, 72);
	}
  else if(color == "fuchsia")
  {
		setColor(255, 0, 255);
	}
  else
  {
		setColor(0, 0, 0);
	}
}

void LedRGB::setColorCA(int crA, int cgA, int cbA){

	analogWrite(_pinR, 255-crA);
	analogWrite(_pinG, 255-cgA);
	analogWrite(_pinB, 255-cbA);
}

void LedRGB::setColorCC(int crC, int cgC, int cbC){
	analogWrite(_pinR, crC);
	analogWrite(_pinG, cgC);
	analogWrite(_pinB, cbC);
}



class LedBar 
{
  public:
   LedBar(int pin_1, int pin_2, int pin_3, int pin_4, int pin_5, int pin_6, int mode = MODE_CC);
   ~LedBar();
   void tunrOn(int pin_led);
   void tunrOff(int pin_led);
   
  private:  
   int _pin_1;
   int _pin_2;
   int _pin_3;
   int _pin_4;
   int _pin_5;
   int _pin_6;
   int _mode;

};

LedBar::LedBar(int pin_1, int pin_2, int pin_3, int pin_4, int pin_5, int pin_6, int mode)
{
	_pin_1 = pin_1;
  _pin_2 = pin_2;
  _pin_3 = pin_3;
  _pin_4 = pin_4;
  _pin_5 = pin_5;
  _pin_6 = pin_6;
	pinMode(_pin_1, OUTPUT);
  pinMode(_pin_2, OUTPUT);
  pinMode(_pin_3, OUTPUT);
  pinMode(_pin_4, OUTPUT);
  pinMode(_pin_5, OUTPUT);
  pinMode(_pin_6, OUTPUT);
	_mode = mode;
}

LedBar::~LedBar()
{

}

void LedBar::tunrOn(int pin_seg)
{
switch (pin_seg)
{
case 1:
  digitalWrite(_pin_1, (_mode == MODE_CC) ? HIGH : LOW);
  break;

case 2:
  digitalWrite(_pin_2, (_mode == MODE_CC) ? HIGH : LOW);
  break;

case 3:
  digitalWrite(_pin_3, (_mode == MODE_CC) ? HIGH : LOW);
  break;

case 4:
  digitalWrite(_pin_4, (_mode == MODE_CC) ? HIGH : LOW);
  break;

case 5:
  digitalWrite(_pin_5, (_mode == MODE_CC) ? HIGH : LOW);
  break;

case 6:
  digitalWrite(_pin_6, (_mode == MODE_CC) ? HIGH : LOW);
  break;

default:
  break;
}
}

void LedBar::tunrOff(int pin_seg)
{
switch (pin_seg)
{
case 1:
  digitalWrite(_pin_1, (_mode == MODE_CC) ? LOW : HIGH);
  break;

case 2:
  digitalWrite(_pin_2, (_mode == MODE_CC) ? LOW : HIGH);
  break;

case 3:
  digitalWrite(_pin_3, (_mode == MODE_CC) ? LOW : HIGH);
  break;

case 4:
  digitalWrite(_pin_4, (_mode == MODE_CC) ? LOW : HIGH);
  break;

case 5:
  digitalWrite(_pin_5, (_mode == MODE_CC) ? LOW : HIGH);
  break;

case 6:
  digitalWrite(_pin_6, (_mode == MODE_CC) ? LOW : HIGH);
  break;

default:
  break;
}
}

//intancias de contructores
Servo servo;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
LedBar ledbar(SEG_1_PIN, SEG_2_PIN, SEG_3_PIN, SEG_4_PIN, SEG_5_PIN, SEG_6_PIN);
LedRGB miLED(SEG_5_PIN, SEG_4_PIN, SEG_6_PIN, CA);
Adafruit_SSD1306 display(128, 64, &Wire, -1);

int distance = 0;

void buttonInterrupt_1()
{
  buzzer_flag = !buzzer_flag;
}

String color = "";
void buttonInterrupt_2()
{
  if (color == "") {
        color = "white";
    } else if (color == "white") {
        color = "red";
    } else if (color == "red") {
        color = "green";
    } else if (color == "green") {
        color = "blue";
    } else if (color == "blue") {
        color = "yellow";
    } else if (color == "yellow") {
        color = "orange";
    } else if (color == "orange") {
        color = "violet";
    } else if (color == "violet") {
        color = "turquoise";
    } else if (color == "turquoise") {
        color = "grey";
    } else if (color == "grey") {
        color = "maroon";
    } else if (color == "maroon") {
        color = "fuchsia";
    } else if (color == "fuchsia") {
        color = "";
    }
}

int monitor = 1;
void buttonInterrupt_3()
{
  monitor = 1;
}

void buttonInterrupt_4()
{
  monitor = 2;
}

void buttonInterrupt_5()
{
  monitor = 3;
}

unsigned long previousMillis = 0;
unsigned long previousMillis_data_refresh = 0;
const long interval = 1000;
const long interval_data_refresh = 100;
uint16_t red;
uint16_t green;
uint16_t blue;
uint16_t clear;
uint16_t ir;
uint16_t mic;
float x;
float y;
float z;



void color_sensor_data(uint16_t r, uint16_t g, uint16_t b, uint16_t clear, uint16_t ir);
void microphone_data(uint16_t mic);
void imu_data(float x, float y, float z);

void setup()
{
  Serial.begin(115200);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }
  servo.attach(SERVO_PIN);
  pinMode(POT_PIN, INPUT);
  pinMode(BUTTON_1_PIN, INPUT_PULLUP);
  pinMode(BUTTON_2_PIN, INPUT_PULLUP);
  pinMode(BUTTON_3_PIN, INPUT_PULLUP);
  pinMode(BUTTON_4_PIN, INPUT_PULLUP);
  pinMode(BUTTON_5_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BUTTON_1_PIN), buttonInterrupt_1, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_2_PIN), buttonInterrupt_2, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_3_PIN), buttonInterrupt_3, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_4_PIN), buttonInterrupt_4, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON_5_PIN), buttonInterrupt_5, FALLING);
}
//Inicializamos la posicion (en grados) del servo

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

  if (currentMillis - previousMillis_data_refresh >= interval_data_refresh) 
  {
    previousMillis_data_refresh = currentMillis;

    distance = sonar.ping_cm();
    angulo = map(analogRead(POT_PIN), 0, 4095, 0, 180);
    red = random(1024);
    green = random(1024);
    blue = random(1024);
    clear = random(1024);
    ir = random(1024);
    mic = random(1024);
    x = random(1024);
    y = random(1024);
    z = random(1024);
  }


  servo.write(angulo);

  if(buzzer_flag)
  {
    //EasyBuzzer.beep(500);
  }
  else
  {
    //EasyBuzzer.beep(0);
  }

 //
  if (distance >= 5) {
    ledbar.tunrOn(1);
  } else {
    ledbar.tunrOff(1);
  }
  
  if (distance > 10) {
    ledbar.tunrOn(2);
  } else {
    ledbar.tunrOff(2);
  }
  
  if (distance > 15) {
    ledbar.tunrOn(3);
  } else {
    ledbar.tunrOff(3);
  }
  
  if (distance > 20) {
    ledbar.tunrOn(4);
  } else {
    ledbar.tunrOff(4);
  }
  
  if (distance > 25) {
    ledbar.tunrOn(5);
  } else {
    ledbar.tunrOff(5);
  }
  
  if (distance > 30) {
    ledbar.tunrOn(6);
  } else {
    ledbar.tunrOff(6);
  }

  miLED.setColor(color);

  if (currentMillis - previousMillis >= interval) 
  {
    previousMillis = currentMillis;
    switch (monitor)
    {
      case 1:
        color_sensor_data(red,green,blue,clear,ir); 
      break;

      case 2:
        microphone_data(mic); 
      break;

      case 3:
        imu_data(x, y, z); 
      break;
    }

  }
  //delay(15);
  
}


void color_sensor_data(uint16_t r, uint16_t g, uint16_t b, uint16_t clear, uint16_t ir) 
{
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("Sensor Color Channels\n"));
  display.print(F("Red: "));
  display.println(r);
  display.print(F("Green: "));
  display.println(g);
  display.print(F("Blue: "));
  display.println(b);
  display.print(F("Clear: "));
  display.println(clear);
  display.print(F("IR: "));
  display.println(ir);
  display.display();
}

void microphone_data(uint16_t mic) 
{
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("     Microphone\n\n\n"));
  display.print(F("Value: "));
  display.println(mic);
  display.display();
}

void imu_data(float x, float y, float z) 
{
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("         IMU\n\n"));
  display.print(F("X axis: "));
  display.println(x);
  display.print(F("Y axis: "));
  display.println(y);
  display.print(F("Z axis: "));
  display.println(z);
  display.display();
}