#use "iROBOsim-v1-1\iROBOsim.ic"

// Global macros (defines).

int WM_LEFT = 0;
int WM_RIGHT = 1;

// Direction of move, forward or backward.

int MD_FORWARD = 0;
int MD_BACKWARD = 1;

// Direction of turn, right or left.

int TD_RIGHT = 0;
int TD_LEFT = 1;

// Global variables.

float pi = 3.1416;

// Wheel base, distance between wheels.

float wheel_base = 12.0;

// Wheel radius in centimeters.

float wheel_radius = (5.6 / 2.0);

// One wheel revolution in centimeters.

float one_wheel_rev = (pi * wheel_radius * 2.0);
float ticks_per_cm = 1100.0 / one_wheel_rev;

//
// PURPOSE: Moves the robot straight in specified direction,
//     forward or backward, at specified velocity for the specified
//     number of ticks.  Function returns when move completed.
//
// RETURN: None.
//
//

void move(int direction, int velocity, long distance)
{
    long distance_ticks = distance;
    int test = MD_BACKWARD;
    
    if (direction == MD_BACKWARD) {
        distance_ticks = -distance;
    }
    rsim_mrp(WM_LEFT, velocity, distance_ticks);
    rsim_mrp(WM_RIGHT, velocity, distance_ticks);
    
    // Wait for move to complete.
    
    rsim_bmd(WM_LEFT);
    rsim_bmd(WM_RIGHT);
}

// PURPOSE: Turns the robot "in place." at tthe specified angle
//     and at specified velocity.  The pivot is performed by
//     using both motors turning in opposite directions. The
//     center of the turn is the midpoint between the two
//     wheels.

void pivot(int direction, int velocity, float angle)
{
    // To be done by student.
}


//
// PURPOSE: Turn robot in specified diretion, left or right,
//     at the specified angle and velocity.  Function returns
//     when turn complete.  The turn is around the left or right
//     wheel depending on direction, one wheel is moved, the
//     other wheel becomes the cneter of the circle the robot is
//     turning in.
//
// RETURN: None.
//

void turn(int direction, int velocity, float angle)
{
    float turn_adj = 1.05;  // Adjustment factor for turn.
    float turn_dist;
    long turn_ticks;
    int wm_moving;  // Specifies which wheel is turned.
    
    turn_dist = ((2.0 * pi * wheel_base) * (angle/360.0));
    turn_dist = turn_dist * turn_adj;;
    printf("Turn distance = %f.\n", turn_dist);
    turn_ticks = (long) (ticks_per_cm * turn_dist);
    printf("Turn ticks = %d.\n", turn_ticks);
    wm_moving = WM_RIGHT;  // Assume right turn.
    if (direction == TD_RIGHT) {
        wm_moving = WM_LEFT;
    }
    rsim_mrp(wm_moving, velocity, turn_ticks);
    rsim_bmd(wm_moving);  // Wait for completion of turn.
}


void main()
{
    int count;
    long dist_ticks;  // Distnace to move forward.
    
    // Initialize robosim.
    
    iROBOinit(RULERWORLD);
    iROBOtrail = 1;  // Draw trail as robot moves.
    printf("One wheel revolution = %f centimeters.\n",
           one_wheel_rev);
    
    dist_ticks = (long) (ticks_per_cm * 90.0);
    
    move(MD_FORWARD, 1000, dist_ticks);
    turn(TD_RIGHT, 200, 90.0);
    move(MD_BACKWARD, 1000, dist_ticks);
    turn(TD_LEFT, 200, 90.0);
    move(MD_FORWARD, 1000, dist_ticks);
    
    printf("Test pivot function.\n");
    
    move(MD_FORWARD, 1000, dist_ticks);
    pivot(TD_RIGHT, 200, 90.0);
    move(MD_FORWARD, 1000, dist_ticks);
    pivot(TD_LEFT, 200, 90.0);
    move(MD_BACKWARD, 1000, dist_ticks);
    
    printf("Program complete.\n");
}