/* motion.c */
/* Rikky, Johanna, and Kyle, 6.270 - IAP 2002 */

void drive(int velocity, float dist) {
    integral = 0.0;
    delta_x = 0.0;
    delta_y = 0.0;
    theta = 0.0;

    servo(STEERING_WHEEL, S0_VERTICAL);

    if (velocity == 0) {
        /* Stop all motion */
        still_moving = FALSE;
        pid_enable = FALSE;
        measure_angle = FALSE;
        desired_clicks = 0.0;
        mod_desired_clicks = 0.0;
        mtr_pwr[LEFT_WHEEL] = 0;
        mtr_pwr[RIGHT_WHEEL] = 0;
    }
    else {
        still_moving = TRUE;

        drive_start_time = seconds();

        desired_clicks = K_CLICKS * ((float)velocity);
        mod_desired_clicks = 0.0;


        desired_distance = fabs(desired_distance);

        desired_distance = dist / INCHES_PER_CLICK;

        if (velocity < 0)
            desired_distance = 0.0 - desired_distance;

        bias = 0.0;
        measure_angle = FALSE;

        if (dist != 0.0)
            measure_distance = TRUE;
        else
            measure_distance = FALSE;

        pid_enable = TRUE;
    }
}

void turbo_eggo () {
    float start_time;

    start_time = seconds();

    still_moving = TRUE;
    kill_process(pid_pid);
    mtr_pwr[LEFT_WHEEL] = 100;
    mtr_pwr[RIGHT_WHEEL] = 100;

    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);

    while ( (!digital(LEFT_LIP_BUMP)) && (!digital(RIGHT_LIP_BUMP)) && 
(seconds() < (start_time + 4.0)) )
        ;
    ao();

    pid_pid = start_process(pid());
    pid_enable = FALSE;
    integral = 0.0;
    desired_clicks = 0.0;
    mod_desired_clicks = 0.0;
    mtr_pwr[LEFT_WHEEL] = 0;
    mtr_pwr[RIGHT_WHEEL] = 0;
    still_moving = FALSE;

}


void turn(int ticks_in_turn, int speed, int direction) {
    int j = 0;
    int i = 0;
    float start_time;

    kill_process(pid_pid);

    speed = abs(speed);
    ticks_in_turn = abs(ticks_in_turn);


    while ((j==0) && (i < 3)){

        servo (STEERING_WHEEL, S0_HORIZONTAL);
        sleep (.25);

        still_moving = TRUE;


        start_time = seconds();

        reset_encoder(LEFT_WHEEL);
        reset_encoder(RIGHT_WHEEL);



        if (direction == CW) {
            mtr_pwr[LEFT_WHEEL] = speed;
            mtr_pwr[RIGHT_WHEEL] = -speed;

            motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
            motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
            motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
            motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);


            while ((read_encoder(LEFT_WHEEL) < ticks_in_turn) && 
(read_encoder(RIGHT_WHEEL) < ticks_in_turn) && ((seconds()-start_time)<2.5)) 
{
                if (read_encoder(LEFT_WHEEL) < read_encoder(RIGHT_WHEEL)) {
                    mtr_pwr[LEFT_WHEEL] += 3;
                    mtr_pwr[RIGHT_WHEEL] += 3;

                    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
                    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);
                }
                else if (read_encoder(LEFT_WHEEL) > 
read_encoder(RIGHT_WHEEL)) {
                    mtr_pwr[LEFT_WHEEL] -= 3;
                    mtr_pwr[RIGHT_WHEEL] -= 3;

                    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
                    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);
                }
            }
        }

        if (direction == CCW) {
            mtr_pwr[LEFT_WHEEL] = -speed;
            mtr_pwr[RIGHT_WHEEL] = speed;

            motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
            motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
            motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
            motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);


            while ((read_encoder(LEFT_WHEEL) < ticks_in_turn) && 
(read_encoder(RIGHT_WHEEL) < ticks_in_turn) && ((seconds()-start_time)<2.5)) 
{
                if (read_encoder(LEFT_WHEEL) < read_encoder(RIGHT_WHEEL)) {
                    mtr_pwr[LEFT_WHEEL] -= 3;
                    mtr_pwr[RIGHT_WHEEL] -= 3;

                    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
                    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);
                }

                else if (read_encoder(LEFT_WHEEL) > 
read_encoder(RIGHT_WHEEL)) {
                    mtr_pwr[LEFT_WHEEL] += 3;
                    mtr_pwr[RIGHT_WHEEL] += 3;

                    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
                    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
                    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);
                }
            }
        }

        j = 1;

        if ((seconds() - start_time) >= 2.5) {
            i++;
            j = 0;
            pid_pid = start_process(pid());
            sleep(.2);
            still_moving = FALSE;
            drive(50, 2.0);
            while (still_moving)
                ;
            kill_process(pid_pid);
            still_moving = TRUE;
            ticks_in_turn = (int)(ticks_in_turn / 2);
        }
    }


    mtr_pwr[LEFT_WHEEL] = 0;
    mtr_pwr[RIGHT_WHEEL] = 0;

    motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
    motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
    motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
    motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);

    pid_pid = start_process(pid());
    still_moving = FALSE;
    drive(0,0.0);
}

void pid() {

    float left_clicks, right_clicks, left_velocity, right_velocity;

    int left_err, right_err;
    float int_err;

    float time, previous_time;

    enable_encoder(LEFT_WHEEL);
    enable_encoder(RIGHT_WHEEL);

    enable_servos();

    previous_time = seconds();

    while (1) {

        /* Ensure that the process runs without interruption */
        hog_processor();

        /* Obtain the system time so that velocities can be computed */
        time = seconds();

        /* Read the values of the shaft encoders and then reset them */
        left_clicks = (float)read_encoder(LEFT_WHEEL);
        reset_encoder(LEFT_WHEEL);
        right_clicks = (float)read_encoder(RIGHT_WHEEL);
        reset_encoder(RIGHT_WHEEL);

        /* The encoders are only single channel, not quadrature. */
        /* Determine which way the wheels are turning by looking at the */
        /* direction that the two motors are supposed to be turning. */
        if (mtr_pwr[LEFT_WHEEL] < 0) {
            left_clicks = 0.0 - left_clicks;
        }
        if (mtr_pwr[RIGHT_WHEEL] < 0) {
            right_clicks = 0.0 - right_clicks;
        }

        /* Compute velocities */
        left_velocity = left_clicks / (time - previous_time);
        right_velocity = right_clicks / (time - previous_time);

        /* Find the distance traveled in both the x, (forward), and y, */
        /* (lateral), directions.  The if statement is used to avoid */
        /* divide by zero errors. */
        if (left_velocity != right_velocity) {
            /*
            delta_y = delta_y - (((WHEEL_BASE * (right_velocity
                + left_velocity)) / (2.0 * (right_velocity
                - left_velocity))) * (cos(((right_clicks - left_clicks)
                / WHEEL_BASE) + theta) - cos(theta))); */
            delta_x = delta_x + (((WHEEL_BASE * (left_velocity
                + right_velocity)) / (2.0 * (right_velocity
                - left_velocity))) * (sin(((right_clicks - left_clicks)
                / WHEEL_BASE) + theta) - sin(theta)));
        }
        else {
            /* delta_y = delta_y + (left_clicks * sin(theta)); */
            delta_x = delta_x + (left_clicks * cos(theta));
        }

        /* If we care about how far the robot has traveled, determine if */
        /* the robot has traveled far enough.  If so, stop the robot. */
        if ((measure_distance && (((desired_distance > 0.0)
                && (delta_x >= desired_distance))
                || ((desired_distance < 0.0)
                && (delta_x <= desired_distance))))){
            still_moving = FALSE;
            pid_enable = FALSE;
            integral = 0.0;
            desired_clicks = 0.0;
            mod_desired_clicks = 0.0;
            mtr_pwr[LEFT_WHEEL] = 0;
            mtr_pwr[RIGHT_WHEEL] = 0;
        }

        if (drive_start_time > (seconds() + 8.0)) {
            still_moving = FALSE;
            pid_enable = FALSE;
            integral = 0.0;
            desired_clicks = 0.0;
            mod_desired_clicks = 0.0;
            mtr_pwr[LEFT_WHEEL] = 0;
            mtr_pwr[RIGHT_WHEEL] = 0;
        }

        /* Ramp up the speed up the robot each time the pid process */
        /* executes by ACCELERATION_FACTOR. */
        if ((desired_clicks > 0.0)
            && (mod_desired_clicks < desired_clicks)) {
                mod_desired_clicks += ACCELERATION_FACTOR;
                limit_float(mod_desired_clicks, 0.0, desired_clicks);
        }

        if ((desired_clicks < 0.0)
            && (mod_desired_clicks > desired_clicks)) {
                mod_desired_clicks -= ACCELERATION_FACTOR;
                limit_float(mod_desired_clicks, desired_clicks, 0.0);
        }


        /* Comptute the integrated error and the proportional errors. */
        int_err = K_INT
            * (integrate (left_clicks, right_clicks, bias));
        left_err = pid_enable
            * (int)(K_PRO * (((mod_desired_clicks * (time - previous_time)) 
- left_clicks) - int_err));
        right_err = pid_enable
            * (int)(K_PRO * (((mod_desired_clicks * (time - previous_time)) 
- right_clicks) + int_err));

        /* Set the power to the motors based on the computed errors */
        mtr_pwr[LEFT_WHEEL] =
            limit_int((mtr_pwr[LEFT_WHEEL] + left_err), -100,100);
        mtr_pwr[RIGHT_WHEEL] =
            limit_int((mtr_pwr[RIGHT_WHEEL] + right_err), -100,100);

        /* Limit the lower bound of the power to the motors so they do */
        /* not stall. */
        if (abs(mtr_pwr[LEFT_WHEEL]) <= 10)
            mtr_pwr[LEFT_WHEEL] = 0;
        if (abs(mtr_pwr[RIGHT_WHEEL]) <= 10)
            mtr_pwr[RIGHT_WHEEL] = 0;

        /* Make the motors move. */
        motor(LEFT_MOTOR_A, mtr_pwr[LEFT_WHEEL]);
        motor(RIGHT_MOTOR_A, mtr_pwr[RIGHT_WHEEL]);
        motor(LEFT_MOTOR_B, mtr_pwr[LEFT_WHEEL]);
        motor(RIGHT_MOTOR_B, mtr_pwr[RIGHT_WHEEL]);

        /* Can't reset the system clock because it is being used for */
        /* other processes.  Therefore, remember the current time so */
        /* the elapsed time can be computed the next time the process */
        /* is executed. */
        previous_time = time;

        /* Release the processor. */
        defer();

        /* Let the motors run for a short while without adjustment. */
        sleep(.1);

    }
}



float integrate (float left_velocity, float right_velocity, float bias) {
    integral += left_velocity - right_velocity + bias;
    integral = limit_float(integral, -1000.0, 1000.0);
    return integral;
}

/* The following are utility math routines which are used above */

/* Floating point modulus division routine */
float fmod (float dividend, float divisor) {
if (dividend > 0.0) {
        while (dividend >= divisor) {
            dividend -= divisor;
        }
    }
    else {
        while (dividend <= -divisor) {
            dividend += divisor;
        }
    }

    return(dividend);
}

float limit_float (float number, float floor, float ceiling) {
    if (number < floor)
        number = floor;
    if (number > ceiling)
        number = ceiling;

    return number;
}


int limit_int (int number, int floor, int ceiling) {
    if (number < floor)
        number = floor;
    if (number > ceiling)
        number = ceiling;

    return number;
}

float fabs (float a) {
    if (a < 0.0)
        return(-a);
    else
        return(a);
}