// Variabili ServoMotori
// Definizione dei pin a cui sono collegati i Servo Motori
#define SERVO_PIN1 9  // ServoMotore 1
#define SERVO_PIN2 10 // ServoMotore 2
// Macro per rinominare OCR1A e OCR1B
#define Vlv1 OCR1A
#define Vlv2 OCR1B

void setup() {
  // Configuro subito i Pin 9 e 10 per il PWM
  config_Servi();
}

// ------------------ METODI GESTIONE SERVI ------------------

// Configurazione dei Pin ServoMotore per il PWM
void config_Servi() {
  // Imposta i pin 9 (PB1) e 10 (PB2) come uscite
  DDRB |= (1 << PB1) | (1 << PB2);  // PB1 è il pin 9, PB2 è il 10 su Arduino Nano
  
  // Configura il Timer1
  TCCR1A = 0; // Resetta TCCR1A
  TCCR1B = 0; // Resetta TCCR1B
  TCNT1  = 0; // Resetta il contatore

  // Imposta il valore del registro ICR1 per ottenere una frequenza di 50Hz
  ICR1 = 39999; // (16*10^6) / (50*8) - 1 = 39999

  // Imposta i registri per il PWM su OC1A (Pin 9) e OC1B (Pin 10)
  TCCR1A |= (1 << WGM11); // Modalità di Fast PWM con ICR1 come top
  TCCR1B |= (1 << WGM13) | (1 << WGM12); // Modalità di Fast PWM con ICR1 come top
  TCCR1A |= (1 << COM1A1) | (1 << COM1B1); // Non-inverting mode su OC1A e OC1B
  
  // Avvia il timer con prescaler di 8
  TCCR1B |= (1 << CS11); 
}

// Calcola i gradi di apertura della Valvola confrontando Tr con Tb
int regola(float Tb, float Tr) {
  float dif = Tb-Tr;
  float venti_perc_Tb = Tb * 0.2;
  uint8_t gradi = 0;
  if (dif > 0) { // se dif == 0 i gradi di apertura sono 0
    if (dif >= venti_perc_Tb) gradi = 180;
    else {
      gradi = dif / venti_perc_Tb * 180;
    }
  }
  // Formula che calcola i valori di OCR1X affinchè l'apertura sia di y gradi.
  return 912 + 22.5778 * gradi;
}

// ------------------ FINE METODI GESTIONE SERVI ------------------

void loop() {
  Vlv1 = regola(21, 18.9);
  Vlv2 = regola(21, 21);
  delay(2000);
}