#include <Servo.h>
#include "SimpleMovingAverage.h"

constexpr int lightLeft = A0;
constexpr int lightRight = A1;
constexpr int servoPin = 9;

Servo headServo;

int heading = 0, filteredHeading = 0;

int convertLightSensorsToHeading(int leftSensor, int rightSensor);
int convertHeadingToServoAngle(int heading);

SimpleMovingAverage headAngleFilter(20);


void setup() {
  // put your setup code here
  Serial.begin(9600);
  headServo.attach(servoPin);
}

void loop() {
  // put your main code here, to run repeatedly:
  heading = convertLightSensorsToHeading(analogRead(lightLeft), analogRead(lightRight));
  filteredHeading = headAngleFilter.filter(heading);
  headServo.write(convertHeadingToServoAngle(filteredHeading));
  Serial.println(filteredHeading);
  //delay(10);
}


int convertLightSensorsToHeading(int leftSensor, int rightSensor) {
  return map((-1 * leftSensor) + rightSensor, -1024, 1023, -90, 90);
}

int convertHeadingToServoAngle(int heading) {
  return heading + 90;
}