#include <Servo.h>
#include "GyverButton.h"

Servo myservo;

#define BTN_F 8    // the number of the pushbutton pin
#define BTN_B 7     // the number of the pushbutton pin
#define BTN_R A3    // the number of the pushbutton pin
#define BTN_L A2     // the number of the pushbutton pin
#define ledPin 13     // the number of the LED pin
#define motorF 6   // the number of the motor forward pin
#define motorB 5    // the number of the motor revers pin


GButton buttonF(BTN_F); //GyverButton pins
GButton buttonB(BTN_B);
GButton buttonR(BTN_R);
GButton buttonL(BTN_L);




#define servoStep 5   // Servo Speed Steps
#define rpmStep 5     // Motor Speed Steps

// variables will change:
byte pos;
byte currentPos;
byte fRpm;
byte bRpm;
byte currentFrwRpm;
byte currentRevRpm;

void setup() {
  myservo.attach(9);              // Servo
  pinMode(ledPin, OUTPUT);         // initialize pins as an output:
  pinMode(motorF, OUTPUT);
  pinMode(motorB, OUTPUT);
  buttonF.setType(HIGH_PULL);      //GyverButton pins
  buttonB.setType(HIGH_PULL);      // initialize the pushbutton pin as an input
  buttonR.setType(HIGH_PULL);
  buttonR.setType(HIGH_PULL);  
  
  Serial.begin(9600);
  Serial.println("Hello World");
  analogWrite(ledPin, 100);
  delay (300);                       // Center position servo
  wheelsStraight();                  // With delay 30s
}

void loop() {
  buttonF.tick();
  buttonB.tick();
  buttonR.tick();
  buttonL.tick();
  buttInputTick();
  motorsServoTick();

}

void buttInputTick() {

  if (buttonF.state() == true && buttonB.state() == false ) {
    Serial.println("Press F");
    motorForward();
    ledOn();
  }
  if (buttonF.isRelease()) {
    currentFrwRpm = 0;
    Serial.println("Release F");

  }
  if (buttonB.state() == true && buttonF.state() == false) {
    Serial.println("Press B");
    motorRevers();
    ledOn();
  }
  if (buttonB.isRelease()) {
    currentRevRpm = 0;
    Serial.println("Release B");
  }

  if (buttonR.state() == true) {
    Serial.println("Press R");
    turnRight();
    ledOn();
  }
  if (buttonR.isRelease()) {
    Serial.println("Release R");
  }
  
  if (buttonL.state() == true) {
    Serial.println("Press L");
    turnLeft();
    ledOn();
  }
  if (buttonL.isRelease()) {

    Serial.println("Release L");
  }
    else {
    // turn LED off:
    
    analogWrite(ledPin, 0);
    analogWrite(motorF, 0);
    analogWrite(motorB, 0);
   wheelsStraight();
   //Serial.println(rpm);

  }

}

void motorsServoTick() {
    analogWrite(motorF, currentFrwRpm);
    analogWrite(motorB, currentRevRpm);
    myservo.write(currentPos);

}

void ledOn() {
  analogWrite(ledPin, 255);
}

void motorForward() { //
  fRpm = currentFrwRpm;
  if (fRpm < 250 ) fRpm = currentFrwRpm + rpmStep; 
  fRpm = constrain(fRpm, 100, 254);
  currentFrwRpm = fRpm;
  Serial.println(fRpm);
  Serial.println(currentFrwRpm);
}
void motorRevers() {  //
  bRpm = currentRevRpm;
  if (bRpm < 250 ) bRpm = currentRevRpm + rpmStep; 
  bRpm = constrain(bRpm, 100, 254);
  currentRevRpm = bRpm;
  Serial.println(bRpm);
  Serial.println(currentRevRpm);
}

void turnLeft() {
    pos = currentPos;
    pos = currentPos - servoStep;
    pos = constrain(pos, 5, 175);
    currentPos = pos;
                // tell servo to go to position in variable 'pos'
    Serial.println(pos);
    Serial.println(currentPos);


}
void turnRight() {
    pos = currentPos;
    pos = currentPos + servoStep;
    pos = constrain(pos, 5, 175);
    currentPos = pos;
                // tell servo to go to position in variable 'pos'
    Serial.println(pos);
    Serial.println(currentPos);
}
void wheelsStraight() {
  //Serial.println(pos); 

  if (pos <= 89) { 
    pos += 1;// goes from 0 degrees to 180 degrees     // in steps of 1 degree
  }

  if (pos >= 91) { // goes from 180 degrees to 0 degrees
    pos -= 1;
  }
  currentPos = pos;
}