/**
 * @(#) $RCSfile$ $Revision$ $Date$
 *
 * Copyright 2003 Orgdot AS. All Rights Reserved.
 * http://dev.swfit.com
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

/**
 * Rotate the leg.
 *
 * @author      Olaf Havnes
 * @version     $Revision$ $Date$
 * @since       SWFIT1.0
 */

// Set the anchor point
anchor_x = _parent.leg_center_radius * eval ("/:COS_" add int (body_angle));
anchor_y = _parent.leg_center_radius * eval ("/:SIN_" add int (body_angle)) + _parent.body_y;

if (touch_ground || grounded)
{
    grounded = true;

    // If we touch the ground, we have to move the foot with the speed of the ground.
    cos_val = leg_x - anchor_x + _parent.GROUND_SPEED;
    sin_val = ground_y - anchor_y - foot_add;

    #include "../_scripts/TrigonometryTableLookup.as"

    // Set the new values
    angle = atan * /:DEG_MULT;

    // Newton's method once
    leg_length = (leg_length + (cos_val * cos_val + sin_val * sin_val) / leg_length) / 2;
    spring_length = leg_length - spring._x;
    spring_speed = 0;

    if (spring_length < spring_equilibrium) _parent.leg_push += eval ("/:SIN_" add int(angle)) * (spring_length - spring_equilibrium);
}
else
{
    angle_acc = eval ("/:COS_" add int (angle)) * LEG_ACC;
    angle_speed += angle_acc - LEG_FR * angle_speed;
    angle += angle_speed;
    while (angle >= /:NUM_DEG) angle -= /:NUM_DEG;
    while (angle < 0) angle += /:NUM_DEG;

    spring_speed += SPRING_ACC * (spring_equilibrium - spring_length) - SPRING_FR * spring_speed;
    spring_length += spring_speed;
}


// Do we touch a peg ?
min_angle = body_angle - _parent.half_swing_angle;
max_angle = body_angle + _parent.half_swing_angle;

// Note that the bounding angles may "change place" when wrapped inside the unit circle.
if (min_angle < 0) min_angle += /:NUM_DEG;
else if (max_angle > /:NUM_DEG) max_angle -= /:NUM_DEG;

// Are we inside our designated area ?
was_inside = inside;
inside = min_angle > max_angle ? 

    angle > min_angle || angle < max_angle :
    angle > min_angle && angle < max_angle;

if (!inside) 
{
    angle = (angle - min_angle) * (angle - min_angle) < (angle - max_angle) * (angle - max_angle) ?

        min_angle :
        max_angle;

    angle_speed = was_inside ? - angle_speed : 0;
    grounded = false;
}

// Compute the x/y coordinates of the foot.
leg_length = spring_length + spring._x;

leg_x = eval ("/:COS_" add int(angle)) * leg_length + anchor_x;
leg_y = eval ("/:SIN_" add int(angle)) * leg_length + anchor_y + foot_add;