#include "BipolarMotor.h"
#include "Devices.h"
#include <math.h>

const int NUM_STEPS = 200;

const int MOTOR_A1 = 8;
const int MOTOR_A2 = 9;
const int MOTOR_B1 = 10;
const int MOTOR_B2 = 11;

const int REVERSE_BUTTON = 2;
const int STEPPING_BUTTON = 3;
const int REVERSE_LED = 4;
const int STEPPING_LED = 5;

const int MAX_SPEED_POT = A0;
const int THROTTLE_POT = A1;

BipolarMotor motor(NUM_STEPS, MOTOR_A1, MOTOR_A2, MOTOR_B1, MOTOR_B2);

void toggleMotorReverse() {
	motor.toggleReverse();
}

void toggleMotorHalfStepping() {
	motor.toggleHalfStepping();
}

Switch reverseSwitch(toggleMotorReverse);
Switch steppingSwitch(toggleMotorHalfStepping);


void setup() {
	pinMode(REVERSE_LED, OUTPUT);
	pinMode(STEPPING_LED, OUTPUT);

	pinMode(REVERSE_BUTTON, INPUT_PULLUP);
	pinMode(STEPPING_BUTTON, INPUT_PULLUP);
}

void loop() {
	float maxSpeed = map(analogRead(MAX_SPEED_POT), 0, 1023, 0, 500) / 100.0f;
	float throttle = map(analogRead(THROTTLE_POT), 0, 1023, 0, 100) / 100.0f;

	reverseSwitch.debounce(!digitalRead(REVERSE_BUTTON));
	steppingSwitch.debounce(!digitalRead(STEPPING_BUTTON));

	digitalWrite(REVERSE_LED, motor.isReverse());
	digitalWrite(STEPPING_LED, motor.isHalfStepping());

	motor.setMaxSpeed(maxSpeed);
	motor.drive(throttle);
}