#include <Adafruit_PWMServoDriver.h>

struct turnout
{
  int closed;
  int thrown;
  int switch_pin;
  int relay_pin;
  int pos;
};

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

const int totDeviatoi = 16;
turnout deviatoi[totDeviatoi];

void setup()
{
  Serial.begin(9600);
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  delay(1000);
  pwm.sleep();
  configura(0, 248, 280, 30, 31);
  configura(1, 225, 311, 32, 33);
  configura(2, 164, 252, 34, 35);
  configura(3, 206, 318, 36, 37);
  configura(4, 283, 339, 38, 39);
  configura(5, 239, 319, 46, 47);
  configura(6, 253, 326, 40, 41);
//  configura(7, 248, 294, 44, 45);
}

void loop()
{
  for(int n = 0; n < totDeviatoi; n++) {
    if(digitalRead(deviatoi[n].switch_pin) == LOW && deviatoi[n].pos < deviatoi[n].thrown){
      pwm.wakeup();
      for (deviatoi[n].pos = deviatoi[n].closed; deviatoi[n].pos <= deviatoi[n].thrown; deviatoi[n].pos += 1) {
        if(deviatoi[n].pos == round((deviatoi[n].thrown + deviatoi[n].closed)/2)){
          digitalWrite(deviatoi[n].relay_pin, LOW);
        }
        pwm.setPWM(n, 0, deviatoi[n].pos);
        Serial.println(deviatoi[n].pos);
        delay(20);
      }
      pwm.sleep();
    }
    if(digitalRead(deviatoi[n].switch_pin) == HIGH && deviatoi[n].pos > deviatoi[n].closed){
      pwm.wakeup();
      for (deviatoi[n].pos = deviatoi[n].thrown; deviatoi[n].pos >= deviatoi[n].closed; deviatoi[n].pos -= 1) {
        if(deviatoi[n].pos == round((deviatoi[n].thrown + deviatoi[n].closed)/2)){
          digitalWrite(deviatoi[n].relay_pin, HIGH);
        }
        pwm.setPWM(n, 0, deviatoi[n].pos);
        Serial.println(deviatoi[n].pos);
        delay(20);
      }
      pwm.sleep();
    }
  }
}

void configura(int addr, int chiuso, int girato, int interruttore, int rele)
{
  deviatoi[addr].closed = chiuso;
  deviatoi[addr].thrown = girato;
  deviatoi[addr].switch_pin = interruttore;
  deviatoi[addr].relay_pin = rele;
  pinMode(deviatoi[addr].switch_pin, INPUT);
  pinMode(deviatoi[addr].relay_pin, OUTPUT);
  if(!digitalRead(deviatoi[addr].switch_pin)){
    deviatoi[addr].pos = deviatoi[addr].thrown;
  } else {
    deviatoi[addr].pos = deviatoi[addr].closed;
    digitalWrite(deviatoi[addr].relay_pin, HIGH);
  }
  Serial.println(deviatoi[addr].pos);
}