/*
 * File: dance.cpp
 * Purpose: A simple dance routine
 */

// First include standard declarations

#include <iostream>
#include "Myro.h"
using namespace std;

// Define the new functions...

void yoyo(double speed, double waitTime) {
    robot.forward(speed, waitTime);
    robot.backward(speed, waitTime);
}

void wiggle(double speed, double waitTime) {
    robot.motors(-speed, speed);
    wait(waitTime);
    robot.motors(speed, -speed);
    wait(waitTime);
    robot.stop();
}

void dance () {
    yoyo(0.5, 0.5);
    yoyo(0.5, 0.5);
    wiggle(0.5,1);
    wiggle(0.5,1);
}

// The main dance program
int main() {
	int count;
	cout << "How many times? ";
	cin >> count;
	while (count > 15 || count < 0) {
		cout << "Legal numbers are 0 to 15.\nTry again: ";
		cin >> count;
	}
    connect();
    cout << "Battery level = " << robot.getBattery() << endl;
    cout << "Running the dance routine...\n";
    for ( int step = 1; step <= count; step = step + 1 ) {
 //   	cout << "Doing dance step " << step << " ... \n";
    	dance();
    }
    cout << "... Done\n";
    disconnect();
}