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

//OLED display settings

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET     -1 
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Buttons settings

#define B_UP 13
#define B_DOWN 27
#define B_LEFT 16
#define B_RIGHT 17

//Slider settings

#define SLIDER_PIN 14

//Joystick settings

#define JOY_X_PIN 25
#define JOY_Y_PIN 23
#define JOY_BUTTON_PIN 26

//Leds settings

#define LED_VERT_PIN 32
#define LED_HORIZ_PIN 33
#define LED_SLIDER_PIN 18
#define LED_SWITCH_PIN 35

//Servos values

int vertServo = 90;
int horizServo = 90;
int sliderValue;


void setup() {
  // put your setup code here, to run once:
  
  //Serial settings
  Serial.begin(9600);

  //OLED display settings
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  display.clearDisplay(); 
  display.setTextSize(1);      
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);     
  display.println("Sistema inicializado...");
  display.display();

  //Pinmode settings

  pinMode(B_UP, INPUT);
  pinMode(B_DOWN, INPUT);
  pinMode(B_LEFT, INPUT);
  pinMode(B_RIGHT, INPUT);
  pinMode(SLIDER_PIN, INPUT);
  pinMode(JOY_X_PIN, INPUT);
  pinMode(JOY_Y_PIN, INPUT);
  pinMode(JOY_BUTTON_PIN, INPUT);
  pinMode(LED_VERT_PIN, OUTPUT);
  pinMode(LED_HORIZ_PIN, OUTPUT);
  pinMode(LED_SLIDER_PIN, OUTPUT);
  pinMode(LED_SWITCH_PIN, OUTPUT);
  
}

void loop() {
  // put your main code here, to run repeatedly:

  //Leds off
  digitalWrite(LED_VERT_PIN, LOW);
  digitalWrite(LED_HORIZ_PIN, LOW);
  digitalWrite(LED_SWITCH_PIN, LOW);

  //OLED DISPLAY CODE

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(15, 0);
  display.print(F("BRAZO ROBOTICO"));

  display.setCursor(0, 12);
  display.print(F("X:")); 
  display.print(vertServo);
  
  display.setCursor(45, 12);
  display.print(F("Y:")); 
  display.print(horizServo);
  
  display.setCursor(85, 12);
  display.print(F("G:")); 
  display.print(sliderValue);

  /*display.setCursor(0, 24);
  display.print(F("BRZ:")); 
  display.print(brazoServoPos);
  
  display.setCursor(65, 24);
  display.print(F("ANT:")); 
  display.print(antebrazoServoPos);*/

  display.display();

  if (digitalRead(B_UP) == HIGH) {
    buttonState("UP");
  }
  else if (digitalRead(B_DOWN) == HIGH) {
    buttonState("DOWN");
  }
  else if (digitalRead(B_LEFT) == HIGH) {
    buttonState("LEFT");
  }
  else if (digitalRead(B_RIGHT) == HIGH) {
    buttonState("RIGHT");
  }

  //Slider code
  sliderState();

}

int buttonState(String buttonType) {
  if (buttonType == "UP") {
    vertServo = vertServo + 5;

    digitalWrite(LED_VERT_PIN, HIGH);
    if (vertServo > 180) {
      vertServo = 180;
    }

    return vertServo;
  }
  else if (buttonType == "DOWN") {
    vertServo = vertServo - 5;
    
    digitalWrite(LED_VERT_PIN, HIGH);
    if (vertServo < 0) {
      vertServo = 0;
    }
    
    return vertServo;
  } 
  else if (buttonType == "LEFT") {
    horizServo = horizServo - 5;
    digitalWrite(LED_HORIZ_PIN, HIGH);
    if (horizServo < 0) {
      horizServo = 0;
    }
    return horizServo;
  } 
  else if (buttonType == "RIGHT") {
    horizServo = horizServo + 5;
    digitalWrite(LED_HORIZ_PIN, HIGH);
    if (horizServo > 180) {
      horizServo = 180;
    }
    return horizServo;
  } 
  return 0;
}

int sliderState() {
  sliderValue = analogRead(SLIDER_PIN);

  sliderValue = map(sliderValue, 0, 4095, 0, 180);
  int ledValue = map(sliderValue, 0, 180, 0, 255);

  analogWrite(LED_SLIDER_PIN, ledValue);

  return sliderValue; }