#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <time.h>
///include <avr/eeprom.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define PinInSpeed 0      //on board: RXI
#define PinInCadence 7    //on board: 7
#define PinInLED 9       //on board: 9

#define PinLedSpeed 5     //on board: 5   WHITE
#define PinLedCadence 6   //on board: 6   ORANGE DEEP SMALL 
#define PinLED 10          //on board: 10   ORANGE BIG
#define PinBuzer 16       //on board: 16   Buzzer

#define ServoPin 4
#define ServoMaxAngle 180
#define MaxAngle 179
#define MinAngle 1

Servo myservo;
int pos = 0;

void calcSpeed() {
  digitalWrite(PinLedSpeed, HIGH);
  delay(1000);
  digitalWrite(PinLedSpeed, LOW);

}

void calcCadence() {
  digitalWrite(PinLedCadence, HIGH);
  delay(1000);
  digitalWrite(PinLedCadence, LOW);
}



void setup() {
  Serial.begin(9600);

  //DIPLAY settings
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.display();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  //INPUT
  pinMode(PinInSpeed, INPUT);
  pinMode(PinInCadence, OUTPUT);
  pinMode(PinInLED, INPUT);

  //Interrupts
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, HIGH);
  attachInterrupt(digitalPinToInterrupt(PinInCadence), calcCadence, RISING);


  //OUTPUT
  pinMode(PinLedSpeed, INPUT);
  pinMode(PinLedCadence, OUTPUT);
  pinMode(PinLED, OUTPUT);
  pinMode(PinBuzer, OUTPUT);

  
  //SERVO
  myservo.attach(ServoPin); // attaches the servo on pin 4 to the servo object
}



void loop() {

  display.clearDisplay();
  display.setCursor(0, 0);

  display.println("LEDY ON 1s ");
  digitalWrite(PinLedSpeed, HIGH);
  digitalWrite(PinLedCadence, HIGH);
  digitalWrite(PinLED, HIGH);
  display.display();
  delay(1000);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("LEDY OFF 1s");
  display.display();
  digitalWrite(PinLedSpeed, LOW);
  digitalWrite(PinLedCadence, LOW);
  digitalWrite(PinLED, LOW);
  delay(1000);

  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("BUZZER test");
  display.display();
  tone(PinBuzer, 500);
  delay(500);
  noTone(PinBuzer);


  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Servo 0");
  display.display();
  myservo.write(0);
  delay(2000);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Servo 180");
  display.display();
  myservo.write(180);
  delay(2000);

  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("W8 4 input");
  display.display();
  delay(1000);


  //
  //  display.display();
  display.clearDisplay();
}