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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#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);

  const int rollPin = A1;  //for testing only.
  const int pitchPin = A0;
  int mappedPitch,mappedRoll,servo1,servo2,pVal,rVal;
  int offset = 50;
  Servo rollServo;
  Servo pitchServo;

void setup() {
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
     for(;;); // Don't proceed, loop forever
  }
  rollServo.attach(5);
  pitchServo.attach(6);
  rollServo.writeMicroseconds(1500);
  pitchServo.writeMicroseconds(1500);
  display.clearDisplay();
}

void loop() {

mappedPitch = map(analogRead(pitchPin),0,1023,1000,2000);
mappedRoll = map(analogRead(rollPin),0,1023,1000,2000);
mappedPitch = mappedPitch - 1500;
mappedRoll = mappedRoll - 1500;
servo1 = mappedPitch + mappedRoll;
servo2 = -mappedPitch + mappedRoll;
servo1 = servo1 + (10 * offset) + 1000;
servo2 = servo2 + (10 * offset) + 1000;

constrain(servo1,1000,2000);
constrain(servo2,1000,2000);

rollServo.writeMicroseconds(servo1);
pitchServo.writeMicroseconds(servo2);

rVal = map(servo1,1000,2000,0,255);
pVal = map(servo2,1000,2000,0,255);

  display.clearDisplay();
  roll_data();
  pitch_data();
  display.display();
  delay(15);
}

void roll_data(){
  
  display.setTextSize(1);
  display.setTextColor(1);
  display.setCursor(5, 0);
  display.print(rVal); 

  display.drawRect(5, 9, 17, 55, WHITE);

  if (rVal<128){
    int x = map(rVal,128,0,36,61);
    for (int i=10; i<17; i++){
      display.drawLine(i, 36 , i, x, 1);
    }
    
   }
  else if (rVal>128){
    int y = map(rVal,128,255,36,10);
    for (int i=10; i<17; i++){
      display.drawLine(i, 36 , i, y, 1);
      
    }
    
  }
   
}
void pitch_data(){

  display.setTextSize(1);
  display.setTextColor(1);
  display.setCursor(5+20, 0);
  display.print(pVal); 

  display.drawRect(5+20, 9, 17, 55, WHITE);

  if (pVal<128){
    int x = map(pVal,128,0,36,61);
    for (int i=10; i<17; i++){
      display.drawLine(i+20, 36 , i+20, x, 1);
    }
   }
  else if (pVal>128){
    int y = map(pVal,128,255,36,10);
    for (int i=10; i<17; i++){
      display.drawLine(i+20, 36 , i+20, y, 1);
    }
  }

}