/* * * * * * * * * * * * * * * * * * * * * * 
 * multi-robot-demoA.c
 * A demo of multiple robots with non-blocking commands in MyroC
 *
 * Description: Three robots go around in cocentric circles, imitating the
 *              solar system for about 30 seconds, after which they
 *               shuffle in variuos directions and beep.
 * Authors: Vasilisa Bashlovkina, Jason Liu
 * Date: September 13, 2014
 *
 * Initial setup: 
 *    Robots are designated 0, 1, and 2, according to 2 arrays:
 *    connectionPorts[] (machine addresses)
 *    robotNames[]      (corresponding robot names)
 *    place three robots all in the same line. 
 *    alignment of robots is specified using "Scribbler-Forward" directions
 *    Robot 0 will be in the center
 *    Robot 1 should be to the left of robot 0, with a separation of 
 *       a robot-distance, and facing the same direction with robot 0. 
 *    Robot 2 should be to the right of robot 0, with a separation of
 *       2.5 robot-distance from the robot 0, and facing the opposite 
 *       direction as Robot 0 or Robot 1.
 *    In summary, Robot 0 should be in middle with robot 1 on its left
 *       and robot 2 on its right.
 *
 * compile with the line
 *  gcc -lbluetooth -ljpeg -lMyroC -o multi-robot-demoA multi-robot-demoA.c
 *
 * * * * * * * * * * * * * * * * * * * * * */

#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include "MyroC.h"


int main(int argc, char **argv)
{

  srand(time(NULL));
  
  int robots[3];
  char * robotNames[] =      {"perlis", 
                              "curry",
                              "wilkes"};
  char * connectionPorts[] = {"B4:D8:A9:00:09:58",
                              "00:1E:19:01:0D:D1",
                              "00:1E:19:01:0E:13"};
 
  
  int i;
  int j;
  int beep_iterations = 10;// * 3 is the duration
  int shuffle_iterations = 8; // random duration (under 15 seconds)
  double seed;
 

  /* Connect to three robots */
  for (i = 0; i < 3; i++)
    {
      robots[i] = rConnect(connectionPorts[i]);
      rSetName (robotNames[i]);
      rBeep(.5, 550 + i * 100);
    }// for connect

 
 
  /* Imitate the solar system */
  for (i = 0; i < 3; i++)
    {
      rSetConnection(robots[i]);
      switch (i)
        {
        case 0: 
          rMotors(1, -1);
          break;
        case 1:
          rMotors(-.4, -.65);
          break;
        case 2:
          rMotors(.74, 1);
          break;
        default:
          break;
        }// switch
    }// for solar system
  
  /* Play music for however much time you want the turning to continue */
  for (i = 0; i < beep_iterations; i++)
    {
      for (j = 0; j < 3; j++)
        {
          rSetConnection(robots[j]);
          rBeep(1, 400 + 100 * i + 33 * j);
        }
    }// for beeping


 /* Random shuffle */
  
  for (i = 0; i < shuffle_iterations; i++)
    {
      for (j = 0; j < 3; j++)
        {
          rSetConnection(robots[j]);
          seed = rand() / (RAND_MAX * 1.0);
         
          if (seed < .5)
            {
              rForward(seed, -1);
              rBeep(seed * .5 ,500 + seed * 700);
              rBeep(seed * .2, 400 + seed * 700);
              rBeep(seed * .3, 600 + seed * 700);
            } // go forward with 60% probability
          else if (seed < .7)
            {
              rTurnLeft(seed, seed/2.0);
              rTurnRight(seed, seed/2.0);
              rForward(.05, -1);
            } // spin with 20% probability
          else 
            {
              rBackward(seed * .5, seed * .5);
              rForward(.1, -1);
            }// go backward with 20% probability
        }
    }// for shuffling

  /* Stop and disconnect */
  for (i = 0; i < 3; i++)
    {
      rSetConnection(robots[i]);
      rStop();
      rDisconnect();
    }// for stop
}// main