//#include <avr/eeprom.h>
//#include <Servo.h>  // model servo: DS3218 PRO
#include <SPI.h>
#include <Wire.h>
#include <time.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define Version "0"
//ostatnia zmiana: zmiana na PCB, dostrajanie
////2DO:



#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 ServoSwitch 4       //on board: 4
#define ServoPin 8          //on board: 8
#define BrakingLight 9      //on board: 9    BrakingLight
#define PinLED 10           //on board: 10   ORANGE loop signal
#define Btn1 18             //on board: A0   Button0
#define Btn2 19             //on board: A1   Button1

#define ServoMaxAngle 130
#define MaxAngle 179
#define MinAngle 1
#define MaxGear 8
#define MinGear 1
#define WheelCircumference  2.130
#define MagnetsCnt 4
#define ms2kmh 3.6
#define Pi 3.1416
#define TimeToSleep 180000 //3 min


//SPEED
int interpt_cnt = 0;


void setup() {
  //SERVO

//
//  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.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setRotation(0);
  display.setTextSize(3);
  display.setCursor(0, 0);
  display.println("Version:");
  display.setCursor(0, 25);
  display.println(Version);
  display.display();
  delay(5);
  //INPUT
  pinMode(PinInSpeed, INPUT_PULLUP); 
  pinMode(9, INPUT_PULLUP); 
  //Interrupts
  attachInterrupt(digitalPinToInterrupt(PinInSpeed), diag, FALLING);
}

void loop() {
  int local_interpt_cnt = 0;
  noInterrupts();
  local_interpt_cnt = interpt_cnt;
  interrupts();
  
  display.clearDisplay();
  display.println("Interupts cnt: ");
  display.setCursor(0, 20);
  display.println(local_interpt_cnt);
  display.setTextSize(2);
  display.display();
  delay(500);
 }

void diag(){
interpt_cnt = interpt_cnt +1;
}