//by Davood Hakemi @electrohakemi with ❤

//Arduino library:
#include <Arduino.h>

//Servo drive library
#include <Servo.h>

#define SERVO_START_PIN 22
#define NUM_SERVOS      32

void showAllSameAngle  (int angle);
void showAllRandomAngle(int start_angle = 0, int end_angle = 181, uint16_t delay_between_servos = 2, uint16_t delay_after_each_op = 150, uint16_t op_count = 15);
void showAllMoveToAngle(int start_angle, int end_angle, uint16_t delay_after_each_op = 6);
void showWave          (uint16_t delay_after_each_step = 40, uint16_t op_count = 6 );
void showCompassPointer(int s);

Servo myServo[NUM_SERVOS];

void setup() 
{
  // Attach pins from the Arduino Mega board to the Servo objects.
  // Starting from pin SERVO_START_PIN, there happen to be exactly 32 pins on the double row pins.
  for( int i=0; i<NUM_SERVOS; i++) {
    myServo[i].attach(i + SERVO_START_PIN);
  }
}

void loop() 
{
  // Sequence one.
  // All servo motor are set to a random angle.
    showAllRandomAngle();

  // Sequence two.
  // All servo motors move with the same angle.
    showAllSameAngle(0); // set to begin position (horn is rotated left)

    // wait to let the viewer get used to it
    delay(1000);                      

    for( int i = 0; i < 3; i++ )
    {
      // move horns to the right
      showAllMoveToAngle(0  , 180, 6); 

      // move horns to the left
      showAllMoveToAngle(180,   0, 6); 
    }

  // Sequence three.
  // A rotating wave.
    showWave();

  // Sequence four.
  // A "compass"
  // Start by pointing upwards
    int pointer = NUM_SERVOS * 3 / 4;

    showCompassPointer(pointer);

    delay(1000);                       // let the viewer get used to new pattern

    for( int i = 0; i < 5; i++ )
    {
      showCompassPointer(--pointer);
      delay(150);
    }

    delay(200);

    for( int i = 0; i < 9; i++)
    {
      showCompassPointer(++pointer);
      delay(150);
    }

    delay(200);

    for( int i = 0; i<5; i++)
    {
      showCompassPointer(--pointer);
      delay(150);
    }

    delay(200);

    for( int i = 0; i<4; i++)
    {
      showCompassPointer(++pointer);
      delay(150);
    }

    delay(160);

    for( int i = 0; i<2; i++)
    {
      showCompassPointer(--pointer);
      delay(150);
    }

    delay(80);

    for( int i = 0; i<1; i++)
    {
      showCompassPointer(++pointer);
      delay(150);
    }

    delay( 2000);
}

// All servo motor are set to a same angle.
void showAllSameAngle(int angle)
{
  for(uint16_t i = 0; i < NUM_SERVOS; i++) {
    myServo[i].write(angle);
  }
}

// All servo motor are set to a random angle.
void showAllRandomAngle(int start_angle, int end_angle, uint16_t delay_between_servos, uint16_t delay_after_each_op, uint16_t op_count)
{
  for(uint16_t j = 0; j < op_count; j++)
  {
    for(uint16_t i = 0; i < NUM_SERVOS; i++)
    {
      myServo[i].write(random(start_angle, end_angle));

      delay(delay_between_servos);
    }
    
    delay(delay_after_each_op);
  }
}

void showAllMoveToAngle(int start_angle, int end_angle, uint16_t delay_after_each_op)
{
  if( end_angle > start_angle )
  {
    for( int i = start_angle; i <= end_angle; i++ )       
    {
      showAllSameAngle(i);

      delay(delay_after_each_op);
    }
  }
  else
  {
    for( int i = start_angle; i >= end_angle; i-- )       
    {
      showAllSameAngle(i);

      delay(delay_after_each_op);
    }  
  }
}

void showWave(uint16_t delay_after_each_step, uint16_t op_count)
{
  for( uint16_t a = 0; a < op_count; a++ )
  {
    for( uint16_t i = 0; i < NUM_SERVOS; i++)
    {
      for( uint16_t j = 0; j < NUM_SERVOS; j++)
      {
        // Calculate distance to active servo
        int d = j - i;

        if( d < 0 )
          d = -d;
        if( d > (NUM_SERVOS / 2) )
          d = NUM_SERVOS - d;

        int angle = 90 - (10 * d);

        if( angle < 0 )
          angle = 0;

        myServo[j].write(angle);
      }

      delay(delay_after_each_step);
    }
  }  
}

// This function makes a "pointer" with the servos.
// It is used to create the "compass".
// The parameter 's' is the servo motor that has the pointer.
// It is allowed that 's' is below zero or larger than the numbers of servo motors.
void showCompassPointer(int s)
{
  int pointerA = (s                   ) % NUM_SERVOS;        // Using the '%' (remainder) for valid number
  int pointerB = (s +                1) % NUM_SERVOS;  // pointer is made with the next servo motor
  int tailA    = (s + NUM_SERVOS/2    ) % NUM_SERVOS;
  int tailB    = (s + NUM_SERVOS/2 + 1) % NUM_SERVOS;

  // make pointer with servo motor s and s+1.
  myServo[pointerA].write(180-56);
  myServo[pointerB].write(56);

  // make tail with servo motor s+16 and s+17.
  myServo[tailA].write(95);
  myServo[tailB].write(85);

  int n = (NUM_SERVOS / 2) - 2;

  // Set servos right of pointer
  int start = pointerB + 1;
  for( int i = 0; i < n; i++ )
  {
    myServo[(start + i) % NUM_SERVOS].write(2);
  }

  // Set servos left of pointer
  start = tailB + 1;
  for( int i = 0; i < n; i++ )
  {
    myServo[(start + i) % NUM_SERVOS].write(178);
  }
}