#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 4




//
const int SW_PIN = 4;
const int PIN_A =2;
const int PIN_B =3;

#include <Encoder.h>

Encoder myEnc(PIN_A, PIN_B);
const int homePosition = 90; 
const int stepValue = 3;
const int servoPin = 9;
#include <Servo.h>

Servo myservo;  
int servoAngel =homePosition;


#include <Wire.h>
//



int relay = 8;
volatile byte relayState = LOW;
long lastDebounceTime = 0;  
long debounceDelay = 10000;


Adafruit_SSD1306 display(SCREEN_WIDTH,SCREEN_HEIGHT, &Wire, OLED_RESET);
int cw = SSD1306_WHITE;
void setup() {
  
  pinMode(SW_PIN, INPUT);
 
   myservo.attach(servoPin); 
   myservo.write(servoAngel);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)){
    while(1);
  }

  

}

long oldPosition  = -999;



void loop() {
  
  long newPosition = myEnc.read();
  if (newPosition != oldPosition) {

    if(newPosition >  oldPosition)
    {
    int newStep = abs(newPosition - oldPosition);     
      servoAngel -= stepValue;
      if(servoAngel <0)
          servoAngel =0;      
      myservo.write(servoAngel); 
      lcdAngel(servoAngel);
    }

    if(newPosition <  oldPosition )
    {
    int newStep = abs(newPosition - oldPosition);     
      servoAngel += stepValue;
      if(servoAngel >180)
          servoAngel =180;
      myservo.write(servoAngel); 
      lcdAngel(servoAngel);
    }
   oldPosition = newPosition;
  }

  if( digitalRead(SW_PIN) == LOW)
  {
    Serial.print("Home: ");
    Serial.println(homePosition);
    servoAngel =homePosition;
      myservo.write(servoAngel); 
      lcdAngel(servoAngel);
  
  }

  //delay(200);
}


void lcdAngel(int angel)
{
  
  int startChar =7;
  for(int i=startChar; i<16; i++)
  {
   
  }
 
   

//
  delay(200);
  display.clearDisplay();
  display.setCursor(0,0);
  display.setTextSize(3.5);
  display.setTextColor(cw);
  display.println("Uhlos:");
  display.println(servoAngel + 3);
  
  display.display();
  delay(5);

}