// BlueROV2 Basic Movement Demo, M. Finn May 2018

#include "eyebot.h"

// Thruster IDs.
const int TOP_LEFT = 1;
const int TOP_RIGHT = 2;
const int FRONT_LEFT = 3;
const int FRONT_RIGHT = 4;
const int BACK_LEFT = 5;
const int BACK_RIGHT = 6;

const int SPEED = 50;

void up(float secs)
{
    MOTORDrive(TOP_LEFT, SPEED);
    MOTORDrive(TOP_RIGHT, SPEED);
    sleep(secs);
    MOTORDrive(TOP_LEFT, 0);
    MOTORDrive(TOP_RIGHT, 0);
}

void down(float secs)
{
    MOTORDrive(TOP_LEFT, -SPEED);
    MOTORDrive(TOP_RIGHT, -SPEED);
    sleep(secs);
    MOTORDrive(TOP_LEFT, 0);
    MOTORDrive(TOP_RIGHT, 0);
}

void forwards(float secs)
{
    MOTORDrive(FRONT_LEFT, SPEED);
    MOTORDrive(FRONT_RIGHT, SPEED);
    MOTORDrive(BACK_LEFT, -SPEED);
    MOTORDrive(BACK_RIGHT, -SPEED);
    sleep(secs);
    MOTORDrive(FRONT_LEFT, 0);
    MOTORDrive(FRONT_RIGHT, 0);
    MOTORDrive(BACK_LEFT, 0);
    MOTORDrive(BACK_RIGHT, 0);
}

void backwards(float secs)
{
    MOTORDrive(FRONT_LEFT, -SPEED);
    MOTORDrive(FRONT_RIGHT, -SPEED);
    MOTORDrive(BACK_LEFT, SPEED);
    MOTORDrive(BACK_RIGHT, SPEED);
    sleep(secs);
    MOTORDrive(FRONT_LEFT, 0);
    MOTORDrive(FRONT_RIGHT, 0);
    MOTORDrive(BACK_LEFT, 0);
    MOTORDrive(BACK_RIGHT, 0);
}

void turn_left(float secs)
{
    MOTORDrive(FRONT_LEFT, -SPEED);
    MOTORDrive(FRONT_RIGHT, SPEED);
    MOTORDrive(BACK_LEFT, SPEED);
    MOTORDrive(BACK_RIGHT, -SPEED);
    sleep(secs);
    MOTORDrive(FRONT_LEFT, 0);
    MOTORDrive(FRONT_RIGHT, 0);
    MOTORDrive(BACK_LEFT, 0);
    MOTORDrive(BACK_RIGHT, 0);
}

void turn_right(float secs)
{
    MOTORDrive(FRONT_LEFT, SPEED);
    MOTORDrive(FRONT_RIGHT, -SPEED);
    MOTORDrive(BACK_LEFT, -SPEED);
    MOTORDrive(BACK_RIGHT, SPEED);
    sleep(secs);
    MOTORDrive(FRONT_LEFT, 0);
    MOTORDrive(FRONT_RIGHT, 0);
    MOTORDrive(BACK_LEFT, 0);
    MOTORDrive(BACK_RIGHT, 0);
}

int main()
{
    float time = 1.0f;

    up(time);
    forwards(time);
    turn_left(time);
    turn_right(time);
    backwards(time);
    down(time);
}
