#include "Servo.h"

struct Ctlr {
  const byte PinServo;
  const byte PinBut;
  int        angNorm;
  int        angRev;

  byte       butState;
  bool       norm;
  Servo      servo;
};

const int  N = 5;
Ctlr ctlr [N] = {
//  Srv  But norm  rev
  {  A0,  13,  40,  120 },
  {  A1,  12,  45,  140 },
  {  A2,  11, 120,   10 },
  {  A3,  10,  90,   60 },
  {  A4,   9,  20,  170 },
};

void setup() {
   Serial.begin(115200);
   Serial.println("button-servo");

  for (int n = 0; n < N; n++)  {
    pinMode (ctlr [n].PinBut, INPUT_PULLUP);
    ctlr [n].butState = digitalRead (ctlr [n].PinBut);

    ctlr [n].servo.attach (ctlr [n].PinServo);
    ctlr [n].servo.write  (ctlr [n].angNorm);
    ctlr [n].norm = true;
  }
}

void
tglServo (
    int n )
{
  if (ctlr [n].norm)  {
    ctlr [n].servo.write (ctlr [n].angRev);
    ctlr [n].norm = false;
    Serial.print("reverse ");
    Serial.println (n);
  }
  else  {
    ctlr [n].servo.write (ctlr [n].angNorm);
    ctlr [n].norm = true;
    Serial.print("normal ");
    Serial.println (n);
  }
}

void chkButtons () {
  for (int n = 0; n < N; n++)  {
    byte but = digitalRead(ctlr [n].PinBut);
  
    if (ctlr [n].butState != but) {   // state change
      ctlr [n].butState  = but;
  
      if (LOW == but)  {              // pressed
        tglServo (n);
      }
    }
  }

  delay (50);     // deounce
}

void loop() {
  chkButtons ();
}