#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <time.h>
///include <avr/eeprom.h>
Adafruit_SSD1306 display(4);

#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;
int spd_cnt = 0;
int spd_cnt_incr = 0;
int cad_cnt = 0;
int cad_cnt_incr = 0;
int i_loop = 0;

void calcSpeed() {
  spd_cnt = spd_cnt + 1;
 // digitalWrite(PinLedSpeed, HIGH);
 // delay(50);
  //digitalWrite(PinLedSpeed, LOW);
Serial.print("spd_cnt_metodaS: ");
Serial.print(spd_cnt);
Serial.println();  
//    if ( spd_cnt >= 2 )
//  {
//
//    spd_cnt_incr = spd_cnt_incr+1;
//    spd_cnt = 0;
//  }

}

void calcCadence() {
  cad_cnt = cad_cnt + 1;
  digitalWrite(PinLedCadence, HIGH);
  delay(50);
  digitalWrite(PinLedCadence, LOW);
Serial.print("spd_cnt_metodaC: ");
Serial.print(cad_cnt);
Serial.println();  
    if ( cad_cnt >= 2 )
  {

    cad_cnt_incr = cad_cnt_incr+1;
    cad_cnt = 0;
  }
 
}



void setup() {
  
  //DIPLAY settings
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setRotation(0);
  display.setTextSize(1);

  //  display.invertDisplay(1);
  //  display.dim(1);

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


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


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

  
  //Interrupts
//   delay(5000);
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), calcSpeed, FALLING);
  //attachInterrupt(digitalPinToInterrupt(PinInCadence), calcCadence, FALLING);
}



void loop() {

  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("SPD_cnt: ");
  
  display.setCursor(50, 0);
  display.println(spd_cnt);

  display.setCursor(0, 10);
  display.println("SPD_inc: ");

  display.setCursor(50, 10);
  display.println(spd_cnt_incr); 
   
  display.display();

Serial.print("spd_cnt: ");
Serial.print(spd_cnt);
Serial.print("spd_cnt_incr: ");
Serial.print(spd_cnt_incr);
Serial.println();  
//digitalWrite(PinLedCadence, HIGH);
//spd_cnt
//spd_cnt_incr
for (i_loop = 0; i_loop < spd_cnt_incr; i_loop++)
{
  digitalWrite(PinLED, HIGH);
  delay(100);
  digitalWrite(PinLED, LOW);
  delay(100);
}

//
//for (i_loop = 0; i_loop < cad_cnt_incr; i_loop++)
//{
//  digitalWrite(PinLED, HIGH);
//  delay(100);
//  digitalWrite(PinLED, LOW);
//  delay(100);
//}
//cad_cnt_incr = 0;


spd_cnt_incr = 0;

  delay(10);

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