#include <movingAvg.h>   
#include <ESP32Servo.h>               

movingAvg Throt(10);
movingAvg Spin(10);
movingAvg bodyRoll(5);
movingAvg headSpin(5);

int CHavg[4];
float throtAvg;
float spinAvg;
float bodyRollAvg;
float headSpinAvg;
const int servoPin1 = 19;
const int servoPin2 = 21;
const int servoPin3 = 22;
const int servoPin4 = 23;
#define VERT_PIN 35
#define HORZ_PIN 34
#define SEL_PIN  33
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
unsigned long startMillis;
unsigned long currentMillis;
const unsigned long period = 60;
int potPin = 33;
int valPotPin;


void setup(){
    Serial.begin(115200);
    Throt.begin();
    Spin.begin();
    bodyRoll.begin();
    headSpin.begin();
    pinMode(VERT_PIN, INPUT);
    pinMode(HORZ_PIN, INPUT);
    pinMode(SEL_PIN, INPUT_PULLUP);
    servo1.attach(servoPin1);
    servo2.attach(servoPin2);
    servo3.attach(servoPin3);
    servo4.attach(servoPin4);
    startMillis = millis();
  
}

void loop(){
  currentMillis = millis();
  valPotPin = analogRead(potPin);
  if (currentMillis - startMillis >= period){
  int horz = analogRead(HORZ_PIN);
  int vert = analogRead(VERT_PIN);
  throtAvg = Throt.reading(horz);
  throtAvg = map(throtAvg,0,4095,0,180);
  spinAvg = Spin.reading(vert);
  spinAvg = map(spinAvg,0,4095,0,180);
  bodyRollAvg = bodyRoll.reading(random(192,1792));
  bodyRollAvg = map(bodyRollAvg,192,1780,0,180);
  headSpinAvg = headSpin.reading(random(192,1792));
  headSpinAvg  = map(headSpinAvg ,192,1780,0,180);
  servo1.write(throtAvg);
  servo2.write(spinAvg);
  servo3.write(bodyRollAvg);
  servo4.write(headSpinAvg);

  
    Serial.print(valPotPin);
    Serial.print(" , ");
    Serial.print(spinAvg );
    Serial.print(" , ");
    Serial.print(bodyRollAvg);
    Serial.print(" , ");
    Serial.print(headSpinAvg);
    Serial.print(" , ");
  Serial.println();
  delay(50);
  startMillis = currentMillis;
  }
}