#include <Servo.h>
#include <Stepper.h>
#include "Drive.h"
#include "Turn.h"
#include "SwordAttack.h"

#define VERMOVE A0
#define HORMOVE A1
#define RIGHTLOCKPIN 3
#define RIGHTTURNPIN 4
#define LEFTTURNPIN 5
#define LEFTLOCKPIN 6
#define FULLFORWARDPIN 10
#define FORWARDPIN 11
#define REVERSEPIN 12
#define FULLREVERSEPIN 13
#define SWORDPIN 2
#define MAX 1024
#define MIN 0

Servo servo;

void setup() { // put your setup code here, to run once:
  Serial.begin(9600);  //servo
  servo.attach(9); //set pin 9 to the servo
  pinMode(SWORDPIN,INPUT_PULLUP);
  servo.write(0); //set servo angle to 0
  
  //test outputs
  pinMode(RIGHTLOCKPIN, OUTPUT);
  pinMode(RIGHTTURNPIN, OUTPUT);
  pinMode(LEFTTURNPIN, OUTPUT);
  pinMode(LEFTLOCKPIN, OUTPUT);
  pinMode(FULLFORWARDPIN, OUTPUT);
  pinMode(FORWARDPIN, OUTPUT);
  pinMode(REVERSEPIN, OUTPUT);
  pinMode(FULLREVERSEPIN, OUTPUT);
  
  //joystick
  pinMode(VERMOVE,INPUT);
  pinMode(HORMOVE,INPUT);
}

void loop() { // put your main code here, to run repeatedly:
  int swordArc=90;
  int y=analogRead(VERMOVE);
  int x=analogRead(HORMOVE);

  //servo move
  swordAttack(servo,swordArc,SWORDPIN);

  //turn left and right
  turnLED(RIGHTTURNPIN,RIGHTLOCKPIN,LEFTTURNPIN,LEFTLOCKPIN,x);

  //move forwards and backwards
  driveLED(FULLREVERSEPIN,REVERSEPIN,FORWARDPIN,FULLFORWARDPIN,y);
  delay(100);

}