/* Arduino  with SSD1306 128x64 I2C OLED display and 10K Ohm pot on A0
 * Carlos Gonçalves
 */
#include <Servo.h> 
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

Servo myservo;  // create servo object to control a servo 
int pos = 150;    // variable to store the servo position 

const int pwmPin = 3; //3 ou 9

#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 I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int testePinA0 = 0;   // teste
int testePinA1 = 0;
int ledPin = 13;      // select the pin for the LED
int sensorValue = 0;  // variable to store the value coming from the sensor

int emg_Threshold =720; // 20 * (5.0 / 1023.0)= 97 mv  40 * (5.0 / 1023.0)= 195 mv
int Threshold_pot = 60; //não mudar só mudar emg_Threshold

long lastDebounceTimeemg1 = 0;  // the last time the output pin was toggled
long lastDebounceTimeemg2 = 0;  // the last time the output pin was toggled
long debounceDelay = 300;    // the debounce time; increase if the output flickers

long lastDebounceTimeteste = 0;

void setup() {
  Serial.begin(9600);
  
 myservo.attach(pwmPin);  // attaches the servo on pin 9 to the servo object 
 delay(10); myservo.write(pos); 

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  // declare the ledPin as an OUTPUT:
  pinMode(ledPin, OUTPUT);
  }
  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("teste ");
  display.setCursor(0,10);             // Start at top-left corner
  display.println("Pot");
  display.setCursor(0,20);             // Start at top-left corner
  display.println("A0 A1");

  display.display();
  delay(1000);


}
// Functions for right alignment of integers
int align2(int q){  // Space in 100s and 10s
  if (q < 100) {display.print(" ");}
    if (q < 10) {display.print(" ");}    
}
int align3(int q){  // Space in 1000s
  if (q < 1000) {display.print(" ");}
  align2(q);
}
// ++++++++++++Main Loop ++++++++++++++
// 1 second timer
void loop() {                   
display.clearDisplay();

//Teste
 //if (testePinA0 == 0 && testePinA1 == 1000) {testePinA0=1000;testePinA1=0;}  
//delay(3000);
 if (testePinA0 == 0 && testePinA1 == 0) {testePinA0=0;testePinA1=1000;lastDebounceTimeteste = millis();}

if ((millis() - lastDebounceTimeteste) > 2000) {testePinA0=1000;testePinA1=0;} 

//A0  Read pot and display values perCent progressbar
  sensorValue = testePinA0; // read the value from the sensor
  int perCentA0 = float(sensorValue * 50.0/1018);
  int perCent2A0 = float(sensorValue * 100.0/1018) ;

drawProgressbar(10,51-perCentA0,10,perCentA0,perCent2A0);
display.setCursor(0,45-(Threshold_pot/2));   
 if (perCent2A0 >= Threshold_pot) {lastDebounceTimeemg1 = millis();}
  //Lógica de verificação do tempo
 if ((millis() - lastDebounceTimeemg1) > debounceDelay) {display.print(" ");}  
 else{if(pos >= 150) {pos -=4;myservo.write(pos);} display.print("M");  } 
//A0
//A1   Read pot and display values perCent progressbar
  sensorValue = testePinA1 ;// read the value from the sensor
  int perCentA1 = float(sensorValue * 50.0/1018);
  int perCent2A1 = float(sensorValue * 100.0/1018) ;

drawProgressbar(40,51-perCentA1,10,perCentA1,perCent2A1);

 display.setCursor(30,45-(Threshold_pot/2));   
 if (perCent2A1 >= Threshold_pot) {lastDebounceTimeemg2 = millis();}
  //Lógica de verificação do tempo
 if ((millis() - lastDebounceTimeemg2) > debounceDelay) {display.print(" ");}  
 else{if(pos <= 170) {pos +=12;myservo.write(pos);} display.print("M"); } 
//A1

display.setTextSize(2); display.setCursor(60,0); display.print(pos);

  display.display();
  delay(10);

}
void drawProgressbar(int x,int y, int width,int height, int progress)
{

  Threshold_pot =map(emg_Threshold , 0, 1023, 0, 100);
  //display.setCursor(0,0); display.print(Threshold_pot);
  display.drawLine(x-3,51-(Threshold_pot/2),x+12,51-(Threshold_pot/2), SSD1306_WHITE);

    display.drawLine(x-2,1,x+11,1, SSD1306_WHITE);
    display.drawLine(x-2,26,x+11,26, SSD1306_WHITE);
    display.drawLine(x-2,51,x+11,51, SSD1306_WHITE);

  display.drawRect(x, 1, 10, 51, SSD1306_WHITE);
  display.fillRect(x, y, width, height , SSD1306_WHITE);

  display.setTextSize(1);               // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);  // Draw white text
  display.setCursor(x-7,55);
  align2(progress);    
  display.print(progress);
  display.println("%");
}