//Code by Graham Gier, USC ID# 7227272017

#include <Myro.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <cstdlib>

using namespace std;

int main()
{
    srand ( time(NULL) );
    double a = 880;
    double b= 987.77;
    double c = 1046.5;
    double d = 1174.66;
    double e = 1318.51;
    double f = 1396.9;
    double g = 1567.98;
    
    connect("/dev/tty.Fluke2-0221-Fluke2");
    cout << robot.getBattery() << endl;
    while(robot.getBattery() > 7 && robot.getLine()[1] == 0 && robot.getLine()[0] ==0)
    {
        cout << "obstacle: "<<endl;
        cout << robot.getObstacle("center") << endl;
        
        cout <<"light:" << endl;
        cout<< robot.getLight("center")<< endl;
        
        //1812 overture
        if (robot.getObstacle("center") > 6000 && robot.getObstacle("left") >6000 && robot.getObstacle("right") > 6000)
        {
            robot.beep(.2, 783);
            robot.beep(.2, 1046.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, 1318.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, c);
            robot.beep(.2, 1174.66);
            robot.beep(.4, e);
            robot.beep(.4, c);
            robot.beep(.8, c);
            wait(.2);
            robot.beep(.2, 783);
            robot.beep(.2, 1046.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, 1318.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, c);
            robot.beep(.2, 1174.66);
            robot.beep(.4, e);
            robot.beep(.4, c);
            robot.beep(.8, c);
            wait(.2);
            robot.beep(.2, a);
            robot.beep(.2, d);
            robot.beep(.2, e);
            robot.beep(.2, d);
            robot.beep(.2, a);
            robot.beep(.2, 698.46);
            robot.beep(.2, a);
            robot.beep(.2, d);
            robot.beep(.2, a);
            robot.beep(.2, 698.46);
            robot.beep(.2, a);
            robot.beep(.8,d);
            wait(.2);
            robot.beep(.2,783.99);
            robot.beep(.2, c);
            robot.beep(.2, d);
            robot.beep(.2, e);
            robot.beep(.2, d);
            robot.beep(.2, c);
            robot.beep(.2, d);
            robot.beep(.4, e);
            robot.beep(.4, c);
            robot.beep(.8, c);
            wait(.2);
            robot.beep(.2, 783);
            robot.beep(.2, 1046.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, 1318.5);
            robot.beep(.2, 1174.66);
            robot.beep(.2, c);
            robot.beep(.2, 1174.66);
            robot.beep(.4, e);
            robot.beep(.4, c);
            robot.beep(.8, c);
            wait(.2);
            robot.beep(.133333, a);
            robot.beep(.133333, a);
            robot.beep(.133333, a);
            robot.beep(.4, c);
            robot.beep(.4, a);
            wait(.4);
            robot.beep(.133333, a);robot.beep(.133333, a);robot.beep(.133333, a);
            robot.beep(.4, b);
            robot.beep(.4, 783.99);
            wait(.4);
            robot.beep(.133333, a);robot.beep(.133333, a);robot.beep(.133333, a);
            robot.beep(.4, 932.33);
            robot.beep(.4, a);
            robot.beep(.4, c);
            robot.beep(.4, b);
            robot.beep(.4, d);
            robot.beep(.4, c);
            robot.beep(.2666666, g);
            robot.beep(.2666666, 1479.98);
            robot.beep(.2666666, f);
            robot.beep(.2666666,e);
            robot.beep(.2666666, 1244.51);
            robot.beep(.2666666,d);
            robot.beep(.2666666, 1108.73);
            robot.beep(.2666666, c);
            robot.beep(.2666666, b);
            robot.beep(.2666666, 932.33);
            robot.beep(.2666666, a);
            robot.beep(.2666666, 830.61);
            robot.beep(.2666666, 783.99);
            wait(.4);
            robot.beep(.4, 830.61);
            wait(.4);
            robot.beep(.4, a);
            wait(.4);
            robot.beep(.4, b);
            wait(.4);
            robot.beep(.2, c);
            robot.beep(.2, 783.99);
            robot.beep(.2, c);
            robot.beep(.2, d);
            robot.beep(.2, e);
            robot.beep(.2, d);
            robot.beep(.2, c);
            robot.beep(.2, d);
            robot.beep(.4, e);
            wait(.4);
            robot.beep(.4, e);
            robot.beep(.6, c);
        }
        
        //stick man
        if(robot.getIR()[1] == 0 || robot.getIR()[0] == 0)
        {
            //legs
            robot.forward(1, 1);
            robot.turnRight(1, 1);
            robot.forward(1, 1);
            robot.turnLeft(1, 1.55);
            robot.forward(1, 1);
            robot.turnRight(1, .3);
            //body
            robot.forward(1, 1.25);
            //arms
            robot.turnRight(1, 1);
            robot.forward(1,.7);
            robot.turnLeft(1, 1.55);
            robot.forward(1,.7);
            robot.turnRight(1, .55);
            robot.turnLeft(1,1);
            robot.forward(1,.7);
            robot.turnRight(1, 1.55);
            robot.forward(1,.7);
            //head
            robot.motors(-.5, 1);
            wait(4);
            robot.motors(0,0);
        }
        
        //surprise
        if(robot.getLight("left") < 60000 || robot.getLight("center") < 55000 || robot.getLight("right") < 60000)
        {
            int randCheck = rand() %10;
            if(randCheck>2)
            {
                int sides = randCheck;
                cout << sides<< endl;
                float angle = ((180-((sides-2)*180/sides))*.00861111111111);
                cout << angle << endl;
                for(int i = 0; i < sides; i++)
                {
                    robot.forward(1,.5);
                    robot.motors(-1,1);
                    wait(angle);
                    robot.motors(0,0);
                }
            }
        }
                

    }
	cout << "Disconnecting" << endl;
	disconnect();
    return 0;
}