#include <Arduino.h>
#include "fun.h"

void setup() {
  //motor
  pinMode(rightPwmPin, OUTPUT);
  pinMode(rightDirPin, OUTPUT);
  pinMode(leftPwmPin, OUTPUT);
  pinMode(leftDirPin, OUTPUT);

  //motor actuator
  pinMode(rightActuatorPin, OUTPUT);
  pinMode(leftActuatorPin, OUTPUT);

  //led
  pinMode(speed1LedPin, OUTPUT);
  pinMode(speed2LedPin, OUTPUT);
  pinMode(speed3LedPin, OUTPUT);
  pinMode(speed4LedPin, OUTPUT);
  pinMode(speed5LedPin, OUTPUT);

  digitalWrite(speed1LedPin, LOW);
  digitalWrite(speed2LedPin, LOW);
  digitalWrite(speed3LedPin, LOW);
  digitalWrite(speed4LedPin, LOW);
  digitalWrite(speed5LedPin, LOW);

  //button
  pinMode(upBtnPin, INPUT_PULLUP);
  pinMode(downBtnPin, INPUT_PULLUP);
  pinMode(hornBtnPin, INPUT_PULLUP);

  digitalWrite(rightActuatorPin, LOW);
  digitalWrite(leftActuatorPin, LOW);

  Serial.begin(9600);

  upBtn.begin();
  downBtn.begin();
  hornBtn.begin();

  upBtn.onPressed(upBtnPressed);
  downBtn.onPressed(downBtnPressed);
  hornBtn.onPressed(hornBtnPressed);
  
  calibrateJoystick();
}

void buttonRead() {
  upBtn.read();
  downBtn.read();
  hornBtn.read();
}

void loop() {
  mapLedToSpeedLevel();
  buttonRead();
  activateMotor();
}