#include <stdint.h>
#include <Arduino.h>
#include <Servo.h>
const int FWD_BTN = 2;
const int REV_BTN = 3;
const int SERVO_PWM = 9;
const int ESC_PWM = 10;
const int ADC_VBatt = A0;
const int ADC_POT_0 = A1;
const int ADC_POT_1 = A2;

Servo esc;
Servo servo;

static float getBattV()
{
    uint16_t raw = (uint16_t)analogRead(ADC_VBatt);
    float vout = (float)raw / 1024.0f * 5.0f;
    return (vout * 2.0f);
}

static float getPot0V()
{
    uint16_t raw = (uint16_t)analogRead(ADC_POT_0);
    return (float)raw / 1024.0f * 5.0f;
}

static float getPot1V()
{
    uint16_t raw = (uint16_t)analogRead(ADC_POT_1);
    return (float)raw / 1024.0f * 5.0f;
}

static void handleMotor()
{
    int val = analogRead(ADC_POT_0);
    val = map(val, 0, 1023, 1100, 1900);
    esc.writeMicroseconds(val);
}

static void handleServo()
{
    int val = analogRead(ADC_POT_1);
    val = map(val, 0, 1023, 1100, 1900);
    servo.writeMicroseconds(val);
}

void setup()
{
    Serial.begin(9600);
    esc.attach(ESC_PWM);
    servo.attach(SERVO_PWM);
}

void loop()
{
    Serial.print("Battery Voltage: ");
    Serial.println(getBattV());
    Serial.print("Fwd Pot: ");
    Serial.print(getPot0V() / 5.0f * 100.0f);
    Serial.println("%");
    Serial.print("Rev Pot: ");
    Serial.print(getPot1V() / 5.0f * 100.0f);
    Serial.println("%");
    delay(500);
}