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

Servo srv; 
float pos = 0.0; 
float pot = 0.0;
int sw1 = 0;
int sw2 = 0;
int step = 0;
int a=0;

#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64
#define OLED_RESET     4 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

static const unsigned char PROGMEM LOGO_LKS_DIY[]  = {
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x01, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x03, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x07, 0xf8, 0x00, 0x00, 0x00, 0x00, 0xf9, 0x8c, 0xbc, 0xfc, 0xb3, 0x26, 0x7f, 0xec, 0x03, 
	0x80, 0x0f, 0xfc, 0x00, 0x30, 0x00, 0x00, 0xcd, 0x87, 0x47, 0x87, 0x33, 0x46, 0x44, 0x8c, 0x03, 
	0x80, 0x1f, 0xfe, 0x00, 0x70, 0x00, 0x00, 0xc5, 0x83, 0x43, 0x33, 0x3b, 0x8f, 0x7c, 0x9e, 0x03, 
	0x80, 0x1f, 0xfe, 0x01, 0xf8, 0x00, 0x00, 0xcd, 0x82, 0x43, 0x3a, 0x7b, 0xcf, 0x7c, 0x9e, 0x03, 
	0x80, 0x0f, 0xff, 0x03, 0xfc, 0x00, 0x00, 0xcd, 0x82, 0x67, 0x9a, 0x4f, 0x69, 0x4c, 0x92, 0x03, 
	0x80, 0x03, 0xff, 0x87, 0xfe, 0x00, 0x00, 0xfb, 0xc2, 0x3c, 0xfa, 0xc7, 0x38, 0xc4, 0xb1, 0x03, 
	0x80, 0x01, 0xff, 0x8f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0xff, 0xc7, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x7f, 0xe3, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x3f, 0xe1, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x1f, 0xf0, 0x7f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x1f, 0xf8, 0x7f, 0xc0, 0x00, 0x00, 0xf9, 0x32, 0x8a, 0x23, 0xdc, 0xe2, 0x00, 0x03, 
	0x80, 0x00, 0x0f, 0xf8, 0x3f, 0xc0, 0x00, 0x00, 0x23, 0x32, 0x8b, 0x22, 0x57, 0xb6, 0x00, 0x03, 
	0x80, 0x00, 0x07, 0xfc, 0x3f, 0xc0, 0x00, 0x00, 0x23, 0xbe, 0x8b, 0xa0, 0x72, 0x36, 0x00, 0x03, 
	0x80, 0x00, 0x03, 0xfc, 0x1f, 0xe0, 0x00, 0x00, 0x27, 0xbe, 0x8a, 0xa0, 0xf2, 0x6f, 0x00, 0x03, 
	0x80, 0x00, 0x03, 0xfc, 0x1f, 0xe0, 0x00, 0x00, 0x27, 0xf2, 0x8a, 0x61, 0xb2, 0x5f, 0x00, 0x03, 
	0x80, 0x00, 0x01, 0xfe, 0x0f, 0xe0, 0x00, 0x00, 0x2c, 0x72, 0x7a, 0x63, 0xdf, 0xf2, 0x00, 0x03, 
	0x87, 0x80, 0x01, 0xfe, 0x0f, 0xe0, 0x00, 0x00, 0x24, 0x62, 0x32, 0x23, 0xcc, 0xf2, 0x00, 0x03, 
	0x87, 0xf0, 0x00, 0xff, 0x8f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x8f, 0xfc, 0x03, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x8f, 0xff, 0x07, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x8f, 0xff, 0x8f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x8f, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x7f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x0f, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x03, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x01, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x7e, 0x1f, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x3f, 0x0f, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x1f, 0x8f, 0xcf, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x0f, 0x8f, 0xc7, 0xff, 0x80, 0x03, 0xf1, 0xef, 0xff, 0xdf, 0x45, 0x3f, 0xc0, 0x03, 
	0x80, 0x70, 0x07, 0x8f, 0xc7, 0xff, 0x80, 0x02, 0x31, 0x19, 0x26, 0x51, 0x65, 0x46, 0x40, 0x03, 
	0x80, 0xf0, 0x03, 0xc7, 0xcf, 0xff, 0x80, 0x03, 0xf1, 0xf0, 0x26, 0xf1, 0xf5, 0xc3, 0x80, 0x03, 
	0x80, 0xf8, 0x03, 0xc7, 0xcf, 0xff, 0xc0, 0x03, 0xf1, 0xd0, 0x27, 0xf1, 0xdd, 0xc0, 0xc0, 0x03, 
	0x80, 0x7c, 0x01, 0xe7, 0xcf, 0xff, 0xc0, 0x02, 0x31, 0x18, 0x26, 0x51, 0x4d, 0x42, 0x40, 0x03, 
	0x80, 0x3e, 0x00, 0xe7, 0xcf, 0xff, 0x80, 0x03, 0xff, 0xef, 0x26, 0x7f, 0x45, 0x3f, 0xc0, 0x03, 
	0x80, 0x3e, 0x00, 0xe7, 0xcf, 0x7f, 0x80, 0x00, 0x00, 0x02, 0x00, 0x04, 0x00, 0x10, 0x80, 0x03, 
	0x80, 0x1f, 0x00, 0x73, 0xcf, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x0f, 0xc0, 0x73, 0xcf, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x07, 0xe0, 0x33, 0xdf, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x03, 0xf0, 0x33, 0xde, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x01, 0xf8, 0x33, 0xde, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x7c, 0x1b, 0xd2, 0xee, 0x00, 0x00, 0x00, 0x00, 0x04, 0x02, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x1f, 0x1b, 0x43, 0xee, 0x00, 0x00, 0x00, 0x80, 0x0c, 0x03, 0x01, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x07, 0x82, 0x41, 0xdc, 0x00, 0x00, 0x03, 0xe0, 0x1e, 0x07, 0x83, 0xc0, 0x00, 0x03, 
	0x80, 0x00, 0x01, 0xe0, 0x00, 0xb8, 0x00, 0x00, 0x03, 0xfe, 0x3f, 0x0f, 0xff, 0xc0, 0x00, 0x03, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x07, 0x7e, 0x73, 0x9c, 0xfe, 0xe0, 0x00, 0x03, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xe7, 0xe1, 0xf8, 0x07, 0xc0, 0x00, 0x03, 
	0x80, 0x84, 0x5c, 0x3a, 0x16, 0x60, 0x00, 0x00, 0x03, 0xe3, 0xc0, 0xf0, 0x03, 0xc0, 0x00, 0x03, 
	0x80, 0x84, 0xb6, 0x6f, 0x1e, 0xc0, 0x00, 0x00, 0x00, 0x01, 0x80, 0x60, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x85, 0x30, 0x63, 0xbf, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x87, 0x1c, 0x3a, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x85, 0x86, 0x0e, 0x5e, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x84, 0xa6, 0x4e, 0x1e, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0xfc, 0x7c, 0x7a, 0x1e, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff 
};

void display_indentitas()
{
  display.clearDisplay();
  display.setCursor(16,10);             
  display.println(F("LKS SMK DIY 2024"));
  display.setCursor(22,30);             
  display.println(F("Abdul Aziz STP"));
  display.setCursor(57,50);             
  display.println(F("00"));
  display.display();
}

void display_logo()
{
  display.clearDisplay();     
  display.drawBitmap(0, 0, LOGO_LKS_DIY, 128, 64, 1);
  display.display();
}

void setup() 
{
  pinMode(A2, INPUT_PULLUP);
  pinMode(A3, INPUT_PULLUP);
  pinMode(A6, INPUT);

  srv.attach(3);
  srv.write(0);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); 
  }
  
  //display.display();
  delay(500);
  display_logo();
  delay(2000);
  //display_indentitas();
  display.clearDisplay();
  display.setCursor(16,10);             
  display.println("LKS SMK DIY 2024");
  display.setCursor(22,30);             
  display.println("Abdul Aziz STP");
  display.setCursor(57,50);             
  display.println("00");
  display.display();
  delay(2000);
}

void loop() 
{
  sw1 = analogRead(A3);
  sw2 = analogRead(A2);
  
  if(sw1 == LOW && sw2 == LOW)
  {
    a++;
    if (a>1)
    {
      a=0;
    }
  }

  if (sw1 == LOW && a == 0)
  {
    step++;
    if (step>=180)
    {
      step = 180;
    }
    pos=step;
    srv.write(pos);
  }
  else if(sw2 == LOW && a == 0)
  {
    step--;
    if (step<0)
    {
      step = 0;
    }
    pos=step;
    srv.write(pos);
  }
  else if(a == 1)
  {
    pot = analogRead(A6);
    pos = map(pot,0,1023,0,180);
    srv.write(pos);
  }

  
  display.clearDisplay();
  display.setTextSize(1);             
  display.setTextColor(SSD1306_WHITE); 
  display.setCursor(2,0);             
  display.print(F("==== SERVO TEST ===="));
  display.setCursor(0,30);             
  display.print(F("Position: "));
  display.print(pos);
  display.display();
}