// ServoOverdone.ino
//
// Example for multiple Servo objects in a array.
//
// Version 1, 28 July 2021 by Koepel
//

#include <Servo.h>

#define NUM_SERVOS 32
Servo myServo[NUM_SERVOS];

void setup() 
{
  // Attach pins from the Arduino Mega board to the Servo objects.
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].attach( i + 22);
  }
}

void loop() 
{
  // All servo motor are set to a random angle.
  for( int a=0; a<15; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      myServo[i].write( random( 0, 181));
    }
    delay( 100);
  }

  // All servo motors move with the same angle.
  for( int a=0; a<4; a++)
  {
    for( int r=0; r<=180; r++)
    {
      for( int i=0; i<NUM_SERVOS; i++)
      {
        myServo[i].write( r);
      }
      delay( 5);
    }
    for( int r=180; r>=0; r--)
    {
      for( int i=0; i<NUM_SERVOS; i++)
      {
        myServo[i].write( r);
      }
      delay( 5);
    }
  }

  // A rotating wave.
  for( int a=0; a<6; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      for( int 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(30);
    }
  }
}

// The function GenerateDiagram() can be used to generate
// the diagram.json file for wokwi.
// To use it, call it from the setup() function, and the
// serial output can be copied into the diagram.json file.
void GenerateDiagram()
{
  Serial.begin(115200);

  Serial.print( "{\n");
  Serial.print( "  \"version\": 1,\n");
  Serial.print( "  \"author\": \"Generated\",\n");
  Serial.print( "  \"editor\": \"wokwi\",\n");
  Serial.print( "  \"parts\": [\n");

  Serial.print( "    {\n");
  Serial.print( "      \"type\": \"wokwi-arduino-mega\",\n");
  Serial.print( "      \"id\": \"mega\",\n");
  Serial.print( "      \"top\": 267.26,\n");
  Serial.print( "      \"left\": 196.02,\n");
  Serial.print( "      \"attrs\": {}\n");
  Serial.print( "    },\n");

  for( int i=0; i<NUM_SERVOS; i++)
  {
    float rotate = float( i) * (360.0 / float( NUM_SERVOS));
    float rad = rotate / 360.0 * 2.0 * M_PI;
    float top = (300.0 * sin( rad)) + 300.0;
    float left = (300.0 * cos( rad)) + 300.0;
    Serial.print( "    {\n");
    Serial.print( "      \"type\": \"wokwi-servo\",\n");
    Serial.print( "      \"id\": \"servo");
    Serial.print( i);
    Serial.print( "\",\n");
    Serial.print( "      \"top\": ");
    Serial.print( top);
    Serial.print( ",\n");
    Serial.print( "      \"left\": ");
    Serial.print( left);
    Serial.print( ",\n");
    Serial.print( "      \"rotate\": ");
    Serial.print( rotate);
    Serial.print( ",\n");
    Serial.print( "      \"attrs\": { \"hornColor\": \"Red\" }\n");
    Serial.print( "    }");
    if( i != NUM_SERVOS - 1)
      Serial.print( ",");
    Serial.print( "\n");
  } 

  Serial.print( "  ],\n");
  Serial.print( "  \"connections\": [\n");

  for( int i=0; i<NUM_SERVOS; i++)
  {
    int j = i + 1;
    if( j == NUM_SERVOS)
      j = 0;
    Serial.print( "    [ \"servo");
    Serial.print( i);
    Serial.print( ":V+\", \"servo");
    Serial.print( j);
    Serial.print( ":V+\", \"Red\", [] ],\n");
    Serial.print( "    [ \"servo");
    Serial.print( i);
    Serial.print( ":GND\", \"servo");
    Serial.print( j);
    Serial.print( ":GND\", \"Black\", [] ],\n");

    Serial.print( "    [ \"mega:");
    Serial.print( i + 22);
    Serial.print( "\", \"servo");
    Serial.print( i);
    Serial.print( ":PWM\", \"Green\", [ ] ],\n");
  }
  Serial.print( "    [ \"mega:GND.2\", \"servo8:GND\", \"Black\", [ ] ],\n");
  Serial.print( "    [ \"mega:5V\", \"servo8:V+\", \"Red\", [ ] ]\n");

  Serial.print( "  ]\n");
  Serial.print( "}\n");
}