//--------------------------------------------------------------
//-- 
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------

#include <Servo.h> 

//-- Mapping between the servo name (in the skymega board) and the
//-- arduino pins
const int SERVO2 = 8;
const int SERVO4 = 9;
const int SERVO6 = 10;
const int SERVO8 = 11;

//-- Array for accesing the 4 servos
Servo myservo[4];  
 
//-- Skymega's test button is attached to pin 12
//--   It has no pull-up resistor
const int BUTTON = 12;
//-- Button states
const int PRESSED = LOW;
const int NOT_PRESSED = HIGH; 
 
//-- Macro for converting from degrees to radians
#define DEG2RAD(g) (g*M_PI)/180

//---- USER parameters -------------------------

//-- Oscillation period (in milliseconds)
const unsigned int T = 2000;

//-- Oscillator's amplitude
const int A[][4]= {
  
// S1  S2  S3  S4     
  {20, 20, 20, 20},  //- 1  
  {20, 20, 20, 20},  //- 2
  {20, 20, 20, 20},  //- 3
};

const int O[][4]={
// S1   S2  S3  S4
   {0, 0, 0, 0},
   {0, 0, 0, 0},
   {0, 0, 0, 0},
};

const double phase0[][4]={
// S1       S2           S3            S4
  {0,       0,           0,             0      },
  {0, -DEG2RAD(45), -DEG2RAD(90), -DEG2RAD(135)},
  {0, -DEG2RAD(90), -DEG2RAD(180), -DEG2RAD(270)}, 
};

//-- Number of movements (rows of the A matrix)
const int M = sizeof(A)/(4*sizeof(int));

//---------------------------------------------


//-- Internal parameters-----------

//-- Sampling period in miliseconds
const unsigned int TS=20;
 
//-- Number of samples per period
const unsigned int N = T/TS; 

//-- Phase's angular increment 
const double inc = 2*M_PI/N; 

//-- This function returns true when the time "time"
//-- has passed since the last call
bool timeout(long time, int timer)
{
  static long previousMillis[2] = {0, 0}; 
  static long currentMillis[2];
  
  //-- Read current time
  currentMillis[timer] = millis();
 
  //-- Reset!
  if (time==0) {
     previousMillis[timer] = currentMillis[timer];
     return true; 
  }
 
  //-- Check if the timeout has passed
  if(currentMillis[timer] - previousMillis[timer] > time) {
    previousMillis[timer] = currentMillis[timer];   

    return true;
  }
  
  return false;
}

//-- This function returns true when the test button
//-- is pressed
bool button_clicked()
{
  
  static int button=1;
  static int oldbutton=1;
  
  //-- Save the previous button state
  oldbutton=button;
  
  // read the button
  button = digitalRead(BUTTON);

  //-- Detect a change in the button
  if (button!=oldbutton) {
    delay(20); //-- for debouncing
    if (button==PRESSED)
      return true;
  } 
  
  return false;
}

 
void setup() 
{ 
  //-- Attaching the 4 servos
  myservo[0].attach(SERVO2);
  myservo[1].attach(SERVO4);
  myservo[2].attach(SERVO6);
  myservo[3].attach(SERVO8);
  
  //-- The button is an input
  pinMode(BUTTON, INPUT);
  //-- Activate the button pull-up resistor
  digitalWrite(BUTTON, HIGH);   
 
  //-- Calculate the initial servo pos
  for (int i=0; i<4; i++) {
    int pos = round(A[0][i] * sin(phase0[0][i]) + O[0][i]);
    myservo[i].write(pos+90);
  }  
 
  //-- Serial port is used for debuging
  Serial.begin(9600); 
  Serial.println(M,DEC);
  
  //-- Wait until the botton is clicked to start the party
  while (!button_clicked());
  
  timeout(0,0);
  timeout(0,1);

} 
 
//-- Internal variables---------------

//-- The phase
double phase=0;
 
//-- Servo pos
int pos; 
  
//-- Current sample
int n=0;

//-- Current movement
int m=0;
  
void update_servos()
{
  //-- Only When TS milliseconds have passed, the new sample is obtained
  if (timeout(TS,0)) {
    
    for (int i=0; i<4; i++) {
      //-- Sample the sine function and set the servo pos
      pos = round(A[m][i] * sin(phase + phase0[m][i]) + O[m][i]);
      myservo[i].write(pos+90);
    }  
   
    //-- Increment the phase
    phase = phase + inc;
   
    //-- Increment the sample index
    n = (n+1) % N;
  }
}
 
void loop() 
{ 
         
  update_servos();
  
  if (button_clicked() || timeout(2*T,1)) {
    timeout(0,1);
    m = (m+1) % M;
  }

}