#include <Servo.h>

//amount of values to collect for everaging
#define AMOUNT_OF_VALUES 5
//minimum angle of the servo movement
#define MINIMUM_POSITION 180
//maximum angle of the servo movement
#define MAXIMUM_POSITION 0
//minimum value of ADC
#define ADC_MIN_VALUE 0
//maximum value of the adc, usually 2^10 for arduinos
#define ADC_MAX_VALUE 1024
//minimum output value of the opamp circuit
#define SENSOR_MIN_VALUE 0
//maximum output value of the opamp circuit. 
//Arduinos have a 0-5V ADC but the current circuit maps to 0-1V for ESP32 chips
#define SENSOR_MAX_VALUE 256
//how long to wait between iterations in the main loop
#define DELAY 200

//pins for the servos
#define SERVO_DAUMEN 2
#define SERVO_ZEIGEFINGER 3
#define SERVO_MITTELFINGER 5
#define SERVO_RINGFINGER 6
#define SERVO_KLEINERFINGER 4

//pins for the sensors
#define SENSOR_DAUMEN A1
#define SENSOR_ZEIGEFINGER A2
#define SENSOR_MITTELFINGER A4
#define SENSOR_RINGFINGER A5
#define SENSOR_KLEINERFINGER A3

Servo servos[5];

/*

|J1- Color   - Use   - Pin    - Comments
|------------|-------|--------|-----------
|1 - Blk     - GND   - GND    -
|2 - Wht     - OUT4  - Pin A2 - Zeigefinger
|3 - NC      -       -        -
|4 - Lgt Brn - OUT5  - Pin A1 - Daumen
|5 - Red     - VCC   - 3.3V   -
|6 - Gry     - OUT3  - Pin A4 - Mittelfinger
|7 - Ppl     - OUT2  - Pin A5 - Ringfinger
|8 - Cyn     - OUT1  - Pin A3 - Kleiner Finger
 */
 
int sensorPins[5] = {SENSOR_DAUMEN, SENSOR_ZEIGEFINGER, SENSOR_MITTELFINGER, SENSOR_RINGFINGER, SENSOR_KLEINERFINGER};
int servoPins[5] = {SERVO_DAUMEN, SERVO_ZEIGEFINGER, SERVO_MITTELFINGER, SERVO_RINGFINGER, SERVO_KLEINERFINGER};

int sensorsRaw[5][AMOUNT_OF_VALUES];
double sensorsAvg[5] = {0,0,0,0,0};
int servoValues[5] = {0,0,0,0,0};

void setup() {

  Serial.begin(9600);

  for(int i = 0; i<5; i++) {
    servos[i].attach(servoPins[i]);
  }
  
}

/*
 * Read the sensors after each other for a total of AMOUNT_OF_VALUES times
 */
void readSensors() {

  for(int i = 0; i<AMOUNT_OF_VALUES; i++) {
    for(int j = 0; j<5; j++) {
      sensorsRaw[j][i] = analogRead(sensorPins[j]);
    }
  }
}

/*
 * Average the AMOUNT_OF_VALUE readings from the sensor
 */
void calculateAvg(){
  for(int i = 0; i<5; i++) {
    for(int j = 0; j<AMOUNT_OF_VALUES; j++) {
      sensorsAvg[i] += sensorsRaw[i][j];
    }
    //calculate average for sensor i
    sensorsAvg[i] /= AMOUNT_OF_VALUES;
  }
}

/*
 * Move servos based on the average value
 */
void moveServos(){
  for(int i = 0; i<5; i++) {
    //make sure the average value is within the bounds of the ADC
    int constrained_value = constrain(sensorsAvg[i], ADC_MIN_VALUE, ADC_MAX_VALUE);
    //linear mapping from sensor values to movement position
    servoValues[i] = map(constrained_value, SENSOR_MIN_VALUE, SENSOR_MAX_VALUE, MINIMUM_POSITION, MAXIMUM_POSITION);
    //move servo i
    servos[i].write(servoValues[i]);
  }
}

/*
 * Debug over serial port, use Serial Plotter in Arduino IDE!
 */
void printDebug() {
  Serial.print("D_Avg:");
  Serial.print(sensorsAvg[0]);
  Serial.print(",");
  Serial.print("ZF_Avg:");
  Serial.print(sensorsAvg[1]);
  Serial.print(",");
  Serial.print("MF_Avg:");
  Serial.print(sensorsAvg[2]);
  Serial.print(",");
  Serial.print("RF_Avg:");
  Serial.print(sensorsAvg[3]);
  Serial.print(",");
  Serial.print("KF_Avg:");
  Serial.println(sensorsAvg[4]);
}

void loop() {
  readSensors();
  calculateAvg();
  moveServos();
  printDebug();
  
  delay(DELAY);
}