#include <Arduino.h>
#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
#include "SPI.h"

#define MIN_PULSE_WIDTH 575
#define MAX_PULSE_WIDTH 2400
#define FREQUENCY 50

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x01);

const int avg = 10;
int pos = 0;
int hist[avg];
int someMath = 0;
int maxRes = 670;
int minRes = 100;
int minPos = 10;
int maxPos = 95;

int pulseWidth(int angle){
  int pulse_wide, analog_value;
  pulse_wide = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  return analog_value;
}

int aread() {
  return analogRead(A0);
}

void setup() {
  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);
  Serial.begin(9600);
  pwm.setPWM(1,0,pulseWidth(0));
	for (int i = 0; i < avg; i++){
		hist[i] = -1;
	}
}

void loop() {
	for (int i = 0; i < avg; i++){
		if(hist[i] == -1){
			hist[i] = aread();
		}
	}
	someMath = 0;
	for (int i = 0; i < avg; i++){
		someMath = someMath + hist[i];
	}
	someMath = someMath / avg;
	pos = map(someMath, minRes, maxRes, maxPos, minPos);
	if(pos < minPos){
		pos = minPos;
	}else if(pos > maxPos){
		pos = maxPos;
	}
	pwm.setPWM(1,0,pulseWidth(pos));
	for (int i = 0; i < avg; i++){
		hist[i] = -1;
	}
  Serial.print(someMath);
  Serial.print(" | ");
  Serial.println(pos);
  delay(10);
}