#include <Servo.h>

//Pin
int pinButtonCalib; //start calibration
int pinButtonTBD; // No function implemented yet.
int pinButtonThumb; // open/close thumb
int pinButtonOther; //lock/unlock other three fingers
int pinServoIndex;
int pinServoOther;
int pinServoThumb;
int pinSensor; //sensor input

//Software
int sensorValue = 0;
int memPouce = 0;
int memIndex = 0;
int mem3Dgts = 0;

//Hardware
Servo servoIndex; //index 
Servo servoOther; //3doigts
Servo servoThumb; //pouce

//Valeurs mini/max servos
const int outThumbMax = 170;//valeur max servo pouce
const int outIndexMax = 142;//valeur max servo index
const int outOtherMax = 96;//valeur max servo 3doigts
 
const int outThumbMin = 170-86;//valeur min servo pouce 170-86=84
const int outIndexMin = 140-117; //valeur min servo index 140-117= 23
const int outOtherMin = 95-55;//valeur min servo 3doigts 95-55 = 40

void setup() {
    Serial.begin(9600);
 
    // Pin Configuration
    pinButtonCalib = A4;// A6 sur Hackberry pour test Calibration -- validation index 
    pinButtonTBD   = A5;// A7 sur Hackberry pour test
    pinButtonThumb = A0;// Verrouillage pouce -- validation pouce
    pinButtonOther = 10;// Verouillage 3 doigts -- validation  3doigts
    pinServoIndex  = 5;// Servo pour l'index 
    pinServoOther  = 6;// Servo pour 3 doigts
    pinServoThumb  = 9;// Servo pour le pouce
    pinSensor      = A1; // Capteur de pression
  
    servoIndex.attach(pinServoIndex);//index
    servoOther.attach(pinServoOther);// 3 doigts
    servoThumb.attach(pinServoThumb);//Pouce
    pinMode(pinButtonCalib, INPUT_PULLUP);
    pinMode(pinButtonTBD,   INPUT_PULLUP);
    pinMode(pinButtonThumb, INPUT_PULLUP);
    pinMode(pinButtonOther, INPUT_PULLUP);
}

void loop() {
  sensorValue = 0; 
  
  if ( digitalRead(pinButtonThumb) == HIGH ){
    memPouce = 1;
    memIndex = 0;
    mem3Dgts = 0;
  }
   if ( digitalRead(pinButtonCalib) == HIGH ){
    memPouce = 0;
    mem3Dgts = 0;
    memIndex = 1;
  }
  if ( digitalRead(pinButtonOther) == HIGH ){
    memPouce = 0;
    mem3Dgts = 1;
    memIndex = 0;
  }
 
  
  if (memPouce == 1){
    sensorValue = analogRead(pinSensor);
    delay(25);
    sensorValue = map(sensorValue, 0, 1023, outThumbMin, outThumbMax);
    Serial.print("sensor pouce= "); Serial.println(sensorValue);
    servoThumb.write(sensorValue);
    delay(15); 
  }
  if (memIndex == 1){
    sensorValue = analogRead(pinSensor);
    delay(25);
    sensorValue = map(sensorValue, 0, 1023,outIndexMin, outIndexMax);
    Serial.print("sensor index= "); Serial.println(sensorValue);
    servoIndex.write(sensorValue);
    delay(15); 
  }
  if (mem3Dgts == 1){
    sensorValue = analogRead(pinSensor);
    delay(25);
    sensorValue = map(sensorValue, 0, 1023,outOtherMin, outOtherMax);
    Serial.print("sensor doigts= "); Serial.println(sensorValue);
    servoOther.write(sensorValue);
    delay(15); 
  }
    
}