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

#include <Servo.h>
#include <Stepper.h>

const int stepsPerRevolution = 200;  
// initialize the stepper library on pins A2 through A5:
Stepper myStepper(stepsPerRevolution, A2, A3, A4, A5);
int steps, msteps, stepsOld = 0;
int oneStep = 5;

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards
int pos = 0;    // variable to store the servo position

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int sensorPin = A0;   // select the input pin for the potentiometer
int ledPin = LED_BUILTIN;      // select the pin for the LED
int sensorValue = 0;  // variable to store the value coming from the sensor
int sensorValueOld = 0;  // variable to store the value coming from the sensor
int servoPin =A1;     //A1 works, 9 or 11 do not work

void setup() {

  Wire.begin();          // PB7 = SDA, PB6 = SCL on Blue Pill

  pinMode(LED_BUILTIN, OUTPUT);
  delay(100);
  digitalWrite(LED_BUILTIN, HIGH);  // change state of the LED by setting the pin to the HIGH voltage level
  
  myStepper.setSpeed(60);

  myservo.attach(servoPin);  // attaches the servo on pin 9 to the servo object

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    while (1);
  }
  digitalWrite(LED_BUILTIN, LOW);  // change state of the LED by setting the pin to the HIGH voltage level

  display.clearDisplay();

  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(18,25);
  display.print("Hi!!!");

  display.display();


}

void loop() {
 /* digitalWrite(LED_BUILTIN, HIGH);  // change state of the LED by setting the pin to the HIGH voltage level
  delay(1000);                      // wait for a second
  digitalWrite(LED_BUILTIN, LOW);   // change state of the LED by setting the pin to the LOW voltage level
  delay(1000); */
 
  sensorValue = analogRead(sensorPin);

if(sensorValue != sensorValueOld) {
   // turn the ledPin on
  digitalWrite(ledPin, HIGH);
  display.clearDisplay();
  display.setCursor(10,25);
  display.print(sensorValue);
 
  pos = sensorValue%180;
  myservo.write(pos);              // tell servo to go to position in variable 'pos'
  delay(100);
  
  /* steps += oneStep;
  if (steps >= 200) {
    msteps = -(steps - oneStep);
    steps = 0;
  } else msteps = oneStep; */
  
  msteps = pos - stepsOld;
  stepsOld = pos;
  myStepper.step(msteps);
  delay(500);

  display.setCursor(60,25);
  display.print(pos);

  display.display();
  
  sensorValueOld = sensorValue;
}
  delay(1000);

  digitalWrite(ledPin, LOW);

}