import static java.lang.Math.*;

/**
 * A WalkAnimation that tries to be a bit more natural than the
 * DumbWalkAnimation. It does so by making the foot follow a sine curve, from
 * which the knee angles are also derived.
 *
 * @author Peter Wu
 */
public class SmarterWalkAnimation implements WalkAnimation {

    /**
     * Length of the upper leg from the groin to the knee.
     */
    private final float legTopLength;

    /**
     * Length of the lower leg from knee to the floor.
     */
    private final float legBottomLength;
    /**
     * Total length of the leg.
     */
    private final double legLength;

    private final double cycleLength;
    private final double MAX_LEG_ANGLE_DEG = 40.0;
    /**
     * Maximum percentage of the legs length to lift the feet.
     */
    private static final double FOOT_MAX_LIFT = .20;
    /**
     * Angles in radians.
     */
    private double leg_angle_left;
    private double leg_angle_right;
    private double knee_angle_left;
    private double knee_angle_right;
    private double bodyOffset;

    SmarterWalkAnimation(float legTopLength, float legBottomLength) {
        this.legTopLength = legTopLength;
        this.legBottomLength = legBottomLength;
        this.legLength = legTopLength + legBottomLength;

        double maxLegAngle = MAX_LEG_ANGLE_DEG * PI / 180;
        // single step = sin(maxLegAngle) * legsLength
        cycleLength = 2 * sin(maxLegAngle) * (legTopLength + legBottomLength);
        System.err.println("Cycle length (2 steps): " + cycleLength + " meter");
    }

    @Override
    public void updatePosition(double pos) {
        // adjust speed
        pos /= cycleLength;
        // whether the robot touches the ground with its left foot
        boolean supported_by_left = pos % 2.0 <= 1.0;
        double t = pos % 1.0;
        assert t >= 0.0 : "Time went negative?!";

        /**
         * At t=0, the body is at x=0; the right foot at x=-1/4 and the left
         * foot at x=1/4. During the transition from t=0 to t=1/2, the right
         * foot accelerates to x=3/4 while the left foot stays at x=-1/4
         * (something has to support the robot while it is moving...).
         */
        Point body = new Point(t / 2.0, 1.0);
        Point foot_l = new Point(.25, 0.0);
        Point foot_r = new Point(.25, 0.0);
        double support_dist;
        if (supported_by_left) {
            foot_r.x = -.25 + t;
            foot_r.y = FOOT_MAX_LIFT * sin(t * PI);
            support_dist = foot_r.x - body.x;
        } else {
            foot_l.x = -.25 + t;
            foot_l.y = FOOT_MAX_LIFT * sin(t * PI);
            support_dist = foot_l.x - body.x;
        }
        // base the robot body bottom position on the foot that is supporting
        // the robot.
        body.y = sqrt(1.0 - support_dist * support_dist);

        // distance between a bent leg and the body
        double dist_leg_r, dist_leg_l;
        dist_leg_l = legLength * distance(body, foot_l);
        dist_leg_r = legLength * distance(body, foot_r);

        // base rotation for legs
        leg_angle_right = calcAngle(body, foot_r);
        leg_angle_left = calcAngle(body, foot_l);
        // bend the knees

        knee_angle_right = cosineRule(legTopLength, legBottomLength, dist_leg_r);
        knee_angle_left = cosineRule(legTopLength, legBottomLength, dist_leg_l);
        // correct the foot position due to the bent leg
        leg_angle_left += cosineRule(legTopLength, dist_leg_l, legBottomLength);
        leg_angle_right += cosineRule(legTopLength, dist_leg_r, legBottomLength);

        bodyOffset = legLength * body.y;
    }

    @Override
    public double getLegAngleLeft() {
        return leg_angle_left * 180 / PI;
    }

    @Override
    public double getLegAngleRight() {
        return leg_angle_right * 180 / PI;
    }

    @Override
    public double getKneeAngleLeft() {
        return knee_angle_left * 180 / PI;
    }

    @Override
    public double getKneeAngleRight() {
        return knee_angle_right * 180 / PI;
    }

    @Override
    public double getArmAngleLeft() {
        return 0;
    }

    @Override
    public double getArmAngleRight() {
        return 0;
    }

    @Override
    public double getBottomOffset() {
        return bodyOffset;
    }

    /*-
     * Given a trangle:
     * y|      a   a
     *  |    / |   | \
     *  |  /___|   |___\
     *  |  b  (c) (c)   b
     * -+------------------- x
     * calculate angle a for length bc and ac. In the left case, the
     * returned angle (in radians) is negative.
     */
    private static double calcAngle(Point a, Point b) {
        double ac = a.y - b.y;
        double cb = b.x - a.x;
        return atan(cb / ac);
    }

    /**
     * Calculates the distance between two points.
     */
    private static double distance(Point a, Point b) {
        double side1 = a.x - b.x;
        double side2 = a.y - b.y;
        return sqrt(side1 * side1 + side2 * side2);
    }

    /**
     * Given a triangle with sides a, b and c, calculate the angle opposite to
     * side c.
     *
     * @return Angle opposed to side c in radians.
     */
    private static double cosineRule(double a, double b, double c) {
        // cosine rule: angle c = arccos( (aa + bb - cc) / 2ab )
        return acos(((a * a) + (b * b) - (c * c)) / (2 * a * b));
    }

    private class Point {

        private double x;
        private double y;

        Point(double x, double y) {
            this.x = x;
            this.y = y;
        }
    }
}