#define MY_CODE
#include<Servo.h>

// TODO: Considera di usare i parametri ufficiali e non quelli empirici.
// Official Servo library params:
// #define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
// #define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
// #define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
// #define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds

void ONDA_QUADRA(byte N, word F, double C)
{
  #ifndef MY_CODE 
  //tone(N, F);
  //
  s.attach(N);
  s.write(0 + 180 * (C - 0.025) / 0.09);
  //
  // analogWrite(N, C * 255);
  #else

  // CONTROLLO DI VALIDITÀ DELL'INPUT
  // Se l'input non è valido non si effettua nessuna modifica
  if((N == 9 || N == 10)  &&  F > 0   &&  (0 <= C && C <= 1)) 
  { 
    // GESTIONE A PARTE DEI CASI PARTICOLARI:
    if(C == 0 || C == 1)
    {
      // Si "slaccia" il pin N:
      if(N == 9) { TCCR1A &= 0b00111111; } else { TCCR1A &= 0b11001111; }  
      // Si procede con la scrittura digitale:
      digitalWrite(N, C);
    }
    else
    {
       // GENERAZIONE DELL'ONDA QUADRA:
       // Allo scopo, si configura la periferica TC1 come seuge.
       //
       // Si impostano il PRESCALER=8 e la MODALITÀ="Fast PWM to ICR1":
       TCCR1A = TCCR1A & 0b11111100 | 0b00000010;
       TCCR1B = 0b00011010;
       // ATTENZIONE! Non bisogna "slacciare" i pin.
       //
       // Si imposta la FREQUENZA e la SOGLIA:
       ICR1 = F_CPU/F/8 - 1;
       if(N == 9) { OCR1A  = ICR1 * C; } else { OCR1B  = ICR1 * C; }
       if(TCNT1 >= ICR1) { TCNT1 = 0; }
       //
       // Si "allaccia" il pin N:
       if(N == 9) { TCCR1A |= 0b10000000; } else { TCCR1A |= 0b00100000; } 
    }
  }

  #endif  
}

word F;
void setup() 
{
    F = 50;
    pinMode( 9, OUTPUT);
    pinMode(10, OUTPUT);
}
 
void loop() 
{
  //MOVIMENTO1();
  MOVIMENTO2();
}

void MOVIMENTO1()
{
  double a;
  for(a = -90;   a <= +90;  a = a + 90)
  {
     ONDA_QUADRA( 9, 50, 0.025 + 0.097 * (+a + 90) / 180); 
     ONDA_QUADRA(10, 50, 0.025 + 0.097 * (-a + 90) / 180);
     delay(1000);
  }
}

void MOVIMENTO2()
{
  
  double a;
  for(a = -90;   a <= +90;  a = a + 1)
  {
     ONDA_QUADRA( 9, 50, 0.025 + 0.097 * (+a + 90) / 180); 
     ONDA_QUADRA(10, 50, 0.025 + 0.097 * (-a + 90) / 180);
     delay(5);
  }
  
  for(a = +90;   a >= -90;  a = a - 1)
  {
     ONDA_QUADRA( 9, 50, 0.025 + 0.097 * (+a + 90) / 180); 
     ONDA_QUADRA(10, 50, 0.025 + 0.097 * (-a + 90) / 180);
     delay(5);
  }
}

// PRIMO PROTOTIPO FUNZIONANTE CON SETUP() E LOOP()
/*
int a, d;

word F;
void setup()
{
  TCCR1A = B10100010; // COM1x clear on match, set on bottom
  TCCR1B = B00011010; // WGM=14, /8 prescaler

  // 2* time stretching factor 
  ICR1 = 40000 - 1;   //20mS period for timer 1
  OCR1A = 3000;       //1.5mS pulse on pin  9 for centre position
  
  DDRB = 0b00000010;
  
  a = 0;
  d = +1;
  
  //   minimal 544 microseconds   OCR1 compare value = 1088
  //   maximum 2400 microseconds  OCR1 compare value = 4800
}

void loop() 
{
  a = a + d;
  OCR1A = map(a, -90, 90, 1000, 5000); // 1088, 4800

  if(a == +90) { d = -1; }
  if(a == -90) { d = +1; }

  delay(20);
}
*/

// PRIMO PROTOTIPO FUNZIONANTE CON MAIN()
/*
int a, d;

int main() 
{
  TCCR1A = B10100010; // COM1x clear on match, set on bottom
  TCCR1B = B00011010; // WGM=14, /8 prescaler

  // 2* time stretching factor 
  ICR1 = 40000 - 1;   //20mS period for timer 1
  OCR1A = 3000;       //1.5mS pulse on pin  9 for centre position
  
  DDRB = 0b00000010;
  
  a = 0;
  d = +1;

  //   minimal 544 microseconds   OCR1 compare value = 1088
  //   maximum 2400 microseconds  OCR1 compare value = 4800  

  while(2 == 2) 
  {
    
    a = a + d;
    OCR1A = map(a, -90, 90, 1000, 5000); // 1088, 4800

    if(a == +90) { d = -1; }
    if(a == -90) { d = +1; }

    _delay_ms(20);
  }

  return 0;
}
*/