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

#include "eyebot.h"

// Thruster IDs.
const int HORIZONTAL = 1;
const int VERTICAL = 2;
const int FRONT = 3;

// Fin servo IDs.
const int FIN_UPPER = 3;
const int FIN_LOWER = 4;

const int SPEED = 50;

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

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

void forwards(float secs)
{
    MOTORDrive(HORIZONTAL, SPEED);
    sleep(secs);
    MOTORDrive(HORIZONTAL, 0);
}

void backwards(float secs)
{
    MOTORDrive(HORIZONTAL, -SPEED);
    sleep(secs);
    MOTORDrive(HORIZONTAL, 0);
}

void left(float secs)
{
    MOTORDrive(FRONT, SPEED);
    sleep(secs);
    MOTORDrive(FRONT, 0);
}

void right(float secs)
{
    MOTORDrive(FRONT, -SPEED);
    sleep(secs);
    MOTORDrive(FRONT, 0);
}

void set_angle(int angle)
{
    int mappedAngle = (int)((angle + 90) * 1.41666);
    SERVOSet(FIN_UPPER, mappedAngle);
    SERVOSet(FIN_LOWER, mappedAngle);
}

int main()
{
    float time = 3.0f;
    int finAngle = 60;
    
    set_angle(-finAngle);
    forwards(time);
    set_angle(finAngle);
    forwards(time);
    sleep(2);
    set_angle(finAngle);
    backwards(time);
    set_angle(-finAngle);
    backwards(time);
}
