#include <LedControl.h>
#include "Controller.h"
#include "Buttons.h"
#include "Inputs.h"

LedControl lc=LedControl(12,11,10,1);
Controller controller(5,6,7);

Buttons buttons(2,3,4);

bool tracking = false;

void setup() {
  // put your setup code here, to run once:

  lc.setDigit(0,3,8,false); //Debug
  lc.setDigit(0,2,8,true);
  lc.setDigit(0,1,8,false); 
  lc.setDigit(0,0,8,false);

  pinMode(13, OUTPUT);

  Serial.begin(9600); //debug
}

void loop() {

  buttons.updateInputs();

  if(buttons.readButton("set")){
    set();
    return;
  }
  if(buttons.readButton("move")){
    move();
    return;
  }

  if(buttons.readButton("motor")){
    Serial.println("Motor");
    controller.setTracking();
    digitalWrite(13,tracking);
    return;
  }

  controller.track();
  delay(25);

}

void move(){
  Inputs input(controller.getXangle(),controller.getYangle(),&buttons);
  input.getInputs();
  
}

void set(){


}