/*
 * The body over time: feet that step, falls, moods and fidgets.
 * Body_update moves everything along; body_pose.c turns it into a skeleton.
 */

#include "people/body_internal.h"

static const float FIDGET_LENGTH[FIDGET_COUNT] = { 0.0f, 1.6f, 2.4f, 2.6f, 2.0f, 3.0f, 6.0f, 2.8f, 2.4f, 5.0f, 5.0f, 5.5f, 5.0f };

static Vec3 clearSpot(const BodyFrame *f, Foot *foot, Vec3 spot, Vec3 other, float reach, bool sloppy, float s);
static Vec3 leapPast(const BodyFrame *f, Foot *foot, Vec3 aim, Vec3 other, float reach, float s);
static void crossOver(Body *b, const BodyFrame *f, float s);
#define KNEE_ROOM   0.35f  /* a foot this far back from anything higher than ... */
#define KNEE_HIGH   0.5f   /* ... this (scaled): room for the knee */
#define CROSS_OVER  0.12f  /* a foot lifted this much over something ... */
#define CROSS_FROM  0.15f  /* ... and this far into its step: half way over it */
#define CROSS_PAST  0.2f   /* the body goes no further than this past where that foot came down */
#define CROSS_DONE  0.06f  /* over: the body this close behind where that foot came down (or past it) */
#define CROSS_AWAY  0.75f  /* the body this far back from it: not going over after all */

static float random01(Body *b) {
    if (b->rng == 0) b->rng = 0x9E3779B9u ^ (unsigned)(b->clock * 7919.0f);
    b->rng ^= b->rng << 13;
    b->rng ^= b->rng >> 17;
    b->rng ^= b->rng << 5;
    return (float)(b->rng & 0xffffffu) / 16777215.0f;
}

static Vec3 rightOf(Vec3 facing) { return vec3_normalize(vec3_cross(facing, UP)); }

/* Turns a horizontal direction towards its right by angle (radians). */
static Vec3 turnRight(Vec3 dir, float angle) {
    return vec3_add(vec3_scale(dir, cosf(angle)), vec3_scale(rightOf(dir), sinf(angle)));
}

/* Step length grows with speed: short steps when slow, long strides when
   running (about 2.9 steps/s at 3 m/s, 4 steps/s at 6.5 m/s). */
static float stepLength(float speed, float s) {
    return clampf(0.3f + 0.27f * speed, 0.35f, 1.8f) * s; /* (a brisk walk: long steps, not a shuffle; a sprint: long strides) */
}

/* How far from the other foot a step over something may land: walking a
   careful step, running a leap. */
static float overReach(const Body *b, float s) {
    return fmaxf(OVER_STRIDE * s, OVER_LEAP * b->step_length);
}

/* Where a foot should stand: beside the body, half a step ahead when moving. */
/* The ground a foot can stand on around the body: no further down than a
   leg reaches (over an edge the foot stays up; the body falls if it must). */
static float groundUnder(const BodyFrame *f, float x, float z, float s) {
    return fmaxf(f->ground(f->ground_ctx, x, z, f->position.y + STEP_UP * s), f->position.y - MAX_DROP * s);
}

static Vec3 footSpot(const Body *b, const BodyFrame *f, int side, float s) {
    float sign = side == 0 ? -1.0f : 1.0f;
    float width = FOOT_X * (1.0f + 0.12f * proudness(b) - 0.2f * shyness(b) + 0.15f * b->style[0]); /* (their own way) */
    Vec3 spot = vec3_add(f->position, vec3_scale(rightOf(f->facing), sign * width * s));
    spot = vec3_add(spot, vec3_scale(f->facing, b->stand_ahead * s)); /* (in front of a seat) */

    Vec3 v = flat(b->velocity);
    float speed = vec3_length(v);
    if (speed > 0.05f) /* half a step ahead, but never further than a leg can land */
        spot = vec3_add(spot, vec3_scale(v, fminf(0.5f * b->step_length, MAX_LEAD * (1.0f + 0.45f * runAmount(speed)) * s) / speed)); /* (running: reaching out further) */

    if (b->unsteadiness > 0.0f) {
        /* Drunk: steps land a bit off */
        float n = sinf(b->clock * 2.3f + (float)side * 1.7f) * cosf(b->clock * 1.1f);
        spot = vec3_add(spot, vec3_scale(rightOf(f->facing), n * 0.15f * b->unsteadiness * s));
    }

    /* Standing about: one foot a little ahead, or the feet wider apart, now and then another way */
    float still = 1.0f - smoothstepf(0.05f, 0.3f, speed);
    if (still > 0.0f) {
        Vec3 st = b->stance[side];
        spot = vec3_add(spot, vec3_scale(vec3_add(vec3_scale(rightOf(f->facing), st.x), vec3_scale(f->facing, st.z)), still * s));
    }

    spot.y = groundUnder(f, spot.x, spot.z, s);
    /* Something higher than a knee just ahead (a wall, a ledge): the foot stays back
       far enough for the knee (it bends forward) */
    for (float d = 0.04f * s; d <= KNEE_ROOM * s; d += 0.04f * s) {
        Vec3 q = vec3_add(spot, vec3_scale(f->facing, d));
        if (f->ground(f->ground_ctx, q.x, q.z, spot.y + 1.2f * s) - spot.y > KNEE_HIGH * s) {
            spot = vec3_sub(spot, vec3_scale(f->facing, KNEE_ROOM * s - d + 0.04f * s));
            spot.y = groundUnder(f, spot.x, spot.z, s);
            break;
        }
    }
    return spot;
}

/* Direction a planted foot points: a little outwards, inwards when shy. */
static Vec3 footDir(const Body *b, Vec3 facing, int side) {
    float sign = side == 0 ? -1.0f : 1.0f;
    return turnRight(facing, sign * (TOE_OUT + 0.04f * b->style[1] - 0.42f * shyness(b)));
}

/* A foot's spot may not cross over to the other foot's side. */
static Vec3 uncrossed(Vec3 spot, const BodyFrame *f, int side, Vec3 other, float s) {
    Vec3 right = rightOf(f->facing);
    float sign = side == 0 ? -1.0f : 1.0f;
    float across = vec3_dot(vec3_sub(spot, other), right) * sign; /* > 0: on its own side */
    if (across < FOOT_GAP * s) {
        spot = vec3_add(spot, vec3_scale(right, sign * (FOOT_GAP * s - across)));
        spot.y = groundUnder(f, spot.x, spot.z, s);
    }
    return spot;
}

static Vec3 ankleAt(Vec3 ground_spot, float s) {
    return vec3_add(ground_spot, vec3(0.0f, ANKLE_Y * s, 0.0f));
}

/* Toe on the ground in front of the ankle, so feet follow slopes. */
static Vec3 toePlanted(const BodyFrame *f, Vec3 ankle, Vec3 dir, float s) {
    Vec3 tip = vec3_add(ankle, vec3_scale(dir, FOOT_LENGTH * s));
    tip.y = f->ground(f->ground_ctx, tip.x, tip.z, ankle.y + 0.1f * s) + 0.02f * s;
    tip.y = fmaxf(tip.y, ankle.y - 0.14f * s); /* toes over an edge stay level */
    return tip;
}

/* "Flat" is the way a planted foot points: the ankle sits higher than the toes. */
#define FLAT_PITCH (-atan2f(ANKLE_Y - 0.02f, FOOT_LENGTH))

/* How high a stepping foot is (0..1) through the step: highest early on
   (sooner the smaller early is), and it leaves and meets the ground gently. */
static float stepArc(float step, float early) {
    return sinf(PI_F * powf(step, early)) * smoothstepf(0.0f, 0.3f, step) * (1.0f - smoothstepf(0.8f, 1.0f, step));
}

/* How far the toes are up when the heel lands (walking; running lands flatter). */
static float heelStrike(float run) { return 0.36f * (1.0f - run); }

/* Swinging foot: it leaves the ground tilted as it was (on the toes), hangs,
   and the toes come up before the heel lands. */
static Vec3 toeSwinging(const BodyFrame *f, Vec3 ankle, Vec3 dir, float step, float pitch0, float strike, float s) {
    float swing = FLAT_PITCH - 0.25f * (1.0f - smoothstepf(0.2f, 0.6f, step)) + strike * smoothstepf(0.45f, 0.95f, step);
    float pitch = lerpf(pitch0, swing, smoothstepf(0.0f, 0.4f, step));
    Vec3 along = vec3_add(vec3_scale(dir, cosf(pitch)), vec3_scale(UP, sinf(pitch)));
    Vec3 tip = vec3_add(ankle, vec3_scale(along, FOOT_LENGTH * s));
    float ground = f->ground(f->ground_ctx, tip.x, tip.z, ankle.y + 0.1f * s) + 0.02f * s;
    if (ankle.y > ground) tip.y = fmaxf(tip.y, ground); /* (coming up from below an edge: no clamping) */
    return tip;
}

static void plantAll(Body *b, const BodyFrame *f, float s) {
    b->root_ok = false; /* (moved elsewhere: the hips are drawn right there) */
    for (int i = 0; i < 2; i++) {
        Foot *foot = &b->feet[i];
        foot->ankle = ankleAt(footSpot(b, f, i, s), s);
        foot->dir = footDir(b, f->facing, i);
        foot->toe = toePlanted(f, foot->ankle, foot->dir, s);
        foot->stepping = false;
    }
}

/* ---------- Falling down, lying, getting up ---------- */

#define LIE_DRAPE     0.5f   /* a falling body can come to lie across things this high (m above the feet) */
#define LIE_TURN_COST 1.2f   /* (per radian off the way the fall wanted to go) */

/* How bad it would be to come to lie this way: across something too high is
   out of the question, lumps under the body are uncomfortable. */
static float lieCost(const Body *b, Vec3 dir, float reach) {
    const float s = b->at_scale;
    const Vec3 feet = b->at, side = rightOf(dir);
    float cost = 0.0f, last = 0.0f;
    for (int k = 0; k <= 12; k++) {
        float along = reach - 0.9f + 0.15f * (float)k; /* from the feet end to the head end (unscaled) */
        for (int w = -1; w <= 1; w++) {
            if (w && (k < 5 || k > 10)) continue; /* (the shoulders and hips are wide) */
            Vec3 p = vec3_add(feet, vec3_add(vec3_scale(dir, along * s), vec3_scale(side, 0.2f * s * (float)w)));
            float h = b->ground(b->clutter_ctx, p.x, p.z, feet.y + 2.5f * s) - feet.y;
            if (h > LIE_DRAPE * s) cost += 20.0f + 20.0f * h;
            if (!w) {
                if (k) cost += 2.0f * fabsf(h - last);
                last = h;
            }
        }
    }
    return cost;
}

/* Where the body comes down: the way it was going if there's room, else a bit
   to the side, or shorter (crumpling at the foot of a wall), never into or
   over something high. */
static void chooseLanding(Body *b, float most_turn) {
    if (!b->ground || b->at_scale <= 0.0f) return;
    static const float TURNS[] = { 0.0f, 0.35f, -0.35f, 0.7f, -0.7f, 1.1f, -1.1f, 1.6f, -1.6f, 2.3f, -2.3f, 3.14f };
    static const float REACHES[] = { 1.0f, 0.7f, 0.45f };
    float best = 1e9f, best_turn = 0.0f, best_reach = 1.0f;
    for (unsigned t = 0; t < sizeof TURNS / sizeof TURNS[0]; t++) {
        if (fabsf(TURNS[t]) > most_turn) continue;
        for (unsigned r = 0; r < sizeof REACHES / sizeof REACHES[0]; r++) {
            float reach = b->lie_offset * REACHES[r];
            float cost = lieCost(b, turnRight(b->fall_dir, TURNS[t]), reach) + LIE_TURN_COST * fabsf(TURNS[t]) +
                         2.0f * (1.0f - REACHES[r]);
            if (cost < best) {
                best = cost;
                best_turn = TURNS[t];
                best_reach = reach;
            }
        }
        if (best < 0.3f) break; /* (good enough: the way it was going) */
    }
    b->fall_dir = turnRight(b->fall_dir, best_turn);
    b->lie_offset = best_reach;
}

void Body_fall(Body *b, Vec3 dir) {
    if (b->pose != POSE_UP) return;
    dir = flat(dir);
    if (vec3_length(dir) < 1e-4f) dir = b->last_facing;
    /* No two falls alike: straight like a plank, knees giving way first, or
       turning on the way down; a bit off the push, a bit faster or slower */
    float r = random01(b);
    b->fall_style = b->unsteadiness > 0.3f && r < 0.5f ? FALL_CRUMPLE : (r < 0.35f ? FALL_PLANK : (r < 0.7f ? FALL_TWIST : FALL_CRUMPLE));
    b->fall_twist = b->fall_style == FALL_TWIST ? (random01(b) - 0.5f) * 1.8f : (random01(b) - 0.5f) * 0.4f;
    b->fall_time  = FALL_TIME * (0.8f + 0.5f * random01(b)) * (b->fall_style == FALL_CRUMPLE ? 1.3f : 1.0f);
    dir = flat(dir);
    if (vec3_length(dir) < 1e-3f) dir = vec3_length(b->last_facing) > 1e-3f ? b->last_facing : vec3(0.0f, 0.0f, 1.0f); /* (no way given: forward) */
    dir = turnRight(vec3_normalize(dir), (random01(b) - 0.5f) * 0.6f);
    b->fall_delay = 0.0f;
    b->pose       = POSE_FALLING;
    b->pose_time  = 0.0f;
    b->fall_dir   = vec3_normalize(dir);
    b->lie_offset = FALL_REACH;
    b->face_down  = vec3_dot(b->fall_dir, b->last_facing) > -0.3f; /* backwards: on the back */
    b->asleep     = false;
    b->down_time  = 1.8f + 1.4f * random01(b);
    b->from_knees = false;
    chooseLanding(b, 3.2f);
    b->face_down  = vec3_dot(b->fall_dir, b->last_facing) > -0.3f;
}

void Body_dropAndRoll(Body *b, Vec3 dir) {
    if (b->pose == POSE_DOWN && !b->rolling) { /* already down: roll right away */
        b->rolling = true;
        b->pose_time = 0.0f;
        b->face_down = true;
        b->down_time = fmaxf(b->down_time, ROLL_REST);
        return;
    }
    if (b->pose != POSE_UP) return;
    Body_fall(b, dir);
    b->fall_style = FALL_DIVE;
    b->fall_twist = 0.0f;
    b->fall_time  = DIVE_TIME;
    b->fall_dir   = vec3_normalize(flat(dir)); /* straight ahead, no wobble */
    b->face_down  = true;
    b->lie_offset = FALL_REACH * 1.05f;
    chooseLanding(b, 1.2f);
    b->down_time  = ROLL_REST;
    b->rolling    = true;
    b->roll       = 0.0f;
    b->roll_side  = 0.0f;
}

void Body_rollOnto(Body *b, Vec3 along, float side) {
    if (b->pose != POSE_UP) return;
    along = flat(along);
    b->pose       = POSE_FALLING;
    b->pose_time  = 0.0f;
    b->fall_dir   = vec3_length(along) > 1e-3f ? vec3_normalize(along) : b->last_facing;
    b->lie_offset = 0.0f;  /* (the hips where it is: on the edge) */
    b->face_down  = true;
    b->asleep     = false;
    b->from_knees = true;
    b->fall_style = FALL_PLANK;
    b->fall_twist = 0.0f;
    b->fall_time  = 0.45f; /* (down onto it, not a fall) */
    b->down_time  = ROLL_OVER_TIME + 0.5f;
    b->rolling    = false;
    b->roll = b->roll_side = 0.0f;
    b->roll_over  = side >= 0.0f ? 1.0f : -1.0f;
}

#define LAND_EASY      5.0f  /* m/s down (about 1.3 m): the legs take it as they are */
#define LAND_DEEP_     7.0f  /* (2.5 m) deep into the knees, bounced off ... */
#define LAND_KNEES     8.2f  /* (3.4 m) ... the knees alone take it up to here (the sure), harder: a roll or a crash */
#define LAND_ROLL_MOST 8.0f  /* (3.3 m) a roll takes it up to this ... */
#define LAND_SPORT     1.0f  /* ... the very sporty this much more (4.1 m) */
#define LAND_BREAK     10.5f /* (5.6 m) landing this hard, something gives (the worst) */
#define ROLL_FORWARD   1.2f  /* m/s along: a roll needs going somewhere */

int Body_land(Body *b, Vec3 dir, float speed, float forward, float sure, float sporty, float *bad) {
    *bad = 0.0f;
    if (b->pose != POSE_UP || speed < LAND_EASY) return LAND_SOFT;
    /* first always into the knees: bounce it off */
    float hard = (speed - LAND_EASY) / (LAND_DEEP_ - LAND_EASY); /* 0 .. 1 at a deep landing */
    b->land_squat = clampf(0.35f + 0.5f * hard, 0.0f, 1.1f);
    b->crouch_v += 3.0f + 2.0f * hard; /* (down at once, not eased) */
    /* going somewhere, not too high, and knowing how: over the shoulder, and up again */
    float limit = LAND_ROLL_MOST + LAND_SPORT * sporty;
    if (speed >= LAND_DEEP_ - 0.5f && forward >= ROLL_FORWARD && speed < limit && random01(b) < sure) {
        float lateral = vec3_dot(flat(dir), rightOf(b->last_facing)); /* (the shoulder on the side the way goes, else their own) */
        float side = fabsf(lateral) > 0.2f ? (lateral > 0.0f ? 1.0f : -1.0f) : (b->quirk[0] > 0.0f ? 1.0f : -1.0f);
        Body_shoulderRoll(b, dir, side);
        *bad = 0.05f * clampf((speed - LAND_DEEP_) / (limit - LAND_DEEP_), 0.0f, 1.0f);
        return LAND_ROLL;
    }
    if (speed < LAND_DEEP_) { *bad = 0.12f * hard * (1.0f - sure); return LAND_DEEP; } /* (the knees take it - an unsure one may twist an ankle) */
    float over = clampf((speed - LAND_DEEP_) / (LAND_BREAK - LAND_DEEP_), 0.0f, 1.0f);
    *bad = clampf(0.2f + 0.8f * over - 0.25f * sure, 0.05f, 1.0f);
    if (speed < LAND_KNEES && random01(b) < 0.4f + 0.5f * sure) { *bad *= 0.6f; return LAND_DEEP; } /* (deep, and still on the feet) */
    Body_fall(b, dir);
    return LAND_CRASH;
}

void Body_shoulderRoll(Body *b, Vec3 dir, float side) {
    if (b->pose != POSE_UP) return;
    Vec3 d = flat(dir);
    b->roll_dir = vec3_length(d) > 1e-3f ? vec3_normalize(d) : b->last_facing;
    b->roll_shoulder = side >= 0.0f ? 1.0f : -1.0f;
    b->shoulder_roll = 1e-4f;
    b->land_squat = 0.0f;
}

bool Body_isRolling(const Body *b) { return b->shoulder_roll > 0.0f; }

float Body_clumsiness(const Body *b) {
    return 0.5f + 0.5f * b->quirk[3];
}

/* Staggers backwards a few steps, then goes down that way. */
static void fallLater(Body *b, Vec3 dir, float delay) {
    if (b->pose != POSE_UP || b->fall_delay > 0.0f) return;
    b->fall_delay = delay;
    b->fall_pending = vec3_normalize(flat(dir));
    b->stumble = STUMBLE_TIME;
    b->landing = CATCH_STEP_TIME;
    b->stumble_push = vec3_add(b->stumble_push, vec3_scale(b->fall_pending, 1.6f));
}

void Body_trip(Body *b, int foot, Vec3 facing, float severity) {
    if (b->pose != POSE_UP || b->stumble > 0.0f || b->fall_delay > 0.0f) return;
    float side = foot == 0 ? -1.0f : 1.0f;
    Vec3 right = rightOf(facing);
    Vec3 lurch = vec3_add(vec3_scale(facing, 0.9f), vec3_scale(right, side * 0.5f));
    b->stumble = STUMBLE_TIME;
    b->landing = CATCH_STEP_TIME;
    b->stumble_push = vec3_add(b->stumble_push, vec3_scale(lurch, 0.6f + 1.2f * severity));
    /* Bad enough: down, over the foot that caught */
    if (random01(b) < (severity - 0.5f) * 0.6f)
        Body_fall(b, vec3_add(facing, vec3_scale(right, side * 0.7f)));
}

/* ---------- Smoking ---------- */

#define CIG_TIME      200.0f  /* seconds it burns (a bit quicker with each drag) */
#define EXHALE_TIME   1.4f

bool Body_smoke(Body *b, bool rolled, int hand) {
    if (b->smoke_phase != SMOKE_NONE || b->pose != POSE_UP) return false;
    b->smoke_phase = rolled ? SMOKE_ROLL : SMOKE_LIGHT;
    b->smoke_t = 0.0f;
    b->cig_left = 1.0f;
    b->cig_hand = hand;
    b->drag = b->exhale = 0.0f;
    b->drag_timer = 4.0f;
    return true;
}

bool Body_isSmoking(const Body *b) {
    return b->smoke_phase != SMOKE_NONE;
}

void Body_stopSmoking(Body *b) {
    if (b->smoke_phase == SMOKE_NONE) return;
    b->butt = b->smoke_phase == SMOKE_ON; /* (half rolled: just put away) */
    b->smoke_phase = SMOKE_NONE;
    b->drag = b->exhale = 0.0f;
}

bool Body_takeButt(Body *b) {
    bool butt = b->butt;
    b->butt = false;
    return butt;
}

static void updateSmoking(Body *b, float speed, float dt) {
    if (b->smoke_phase == SMOKE_NONE) return;
    if (b->pose != POSE_UP) { Body_stopSmoking(b); return; } /* (down: out of the hand) */
    b->smoke_t += dt;
    b->exhale = fmaxf(0.0f, b->exhale - dt);
    if (b->smoke_phase == SMOKE_ROLL) {
        if (b->hand_weight[0] > 0.5f || b->hand_weight[1] > 0.5f || speed > 1.0f) { b->smoke_t = fminf(b->smoke_t, 0.5f); return; } /* (hands needed: rolling waits) */
        if (b->smoke_t >= ROLL_CIG_TIME) { b->smoke_phase = SMOKE_LIGHT; b->smoke_t = 0.0f; }
        return;
    }
    if (b->smoke_phase == SMOKE_LIGHT) {
        if (b->smoke_t >= LIGHT_TIME) { b->smoke_phase = SMOKE_ON; b->smoke_t = 0.0f; b->exhale = EXHALE_TIME; }
        return;
    }
    float hold = speed < 0.5f && b->cig_hand < 2 ? 1.0f : 0.0f; /* (standing about: up in front; walking: it swings along) */
    b->cig_hold += clampf(hold - b->cig_hold, -2.5f * dt, 2.5f * dt);
    /* In a hand that's needed for something: into the mouth; back to a free hand after a drag */
    if (b->cig_hand < 2 && b->hand_weight[b->cig_hand] > 0.3f) b->cig_hand = 2;
    b->cig_left -= dt / CIG_TIME * (b->drag > 0.0f ? 4.0f : 1.0f);
    if (b->drag > 0.0f && (b->drag -= dt) <= 0.0f) {
        b->drag = 0.0f;
        b->exhale = EXHALE_TIME;
        if (b->cig_hand == 2)
            for (int h = 1; h >= 0; h--) if (b->hand_weight[h] < 0.05f) { b->cig_hand = h; break; }
    }
    if (b->drag <= 0.0f && (b->drag_timer -= dt) <= 0.0f) {
        b->drag = DRAG_TIME;
        b->drag_timer = 8.0f + 12.0f * random01(b) - 4.0f * b->shown.nerves; /* (nervous: more often) */
    }
    if (b->cig_left <= 0.0f) { b->butt = true; b->smoke_phase = SMOKE_NONE; b->drag = 0.0f; }
}

bool Body_takeLanding(Body *b, int *foot, Vec3 *at) {
    if (!b->landed) return false;
    *foot = b->landed - 1;
    *at = b->landed_at;
    b->landed = 0;
    return true;
}

void Body_stagger(Body *b, Vec3 push) {
    if (b->pose != POSE_UP) return;
    b->stumble = STUMBLE_TIME;
    b->landing = CATCH_STEP_TIME;
    b->stumble_push = vec3_add(b->stumble_push, push);
}

void Body_lieDown(Body *b) {
    b->asleep = true;
    b->down_time = -1.0f;
    if (b->pose != POSE_UP) return; /* already on the ground: just stay there */
    b->pose       = POSE_LYING_DOWN;
    b->pose_time  = 0.0f;
    b->fall_dir   = vec3_scale(b->last_facing, -1.0f); /* sits down, then lies back */
    if (vec3_length(b->fall_dir) < 1e-3f) b->fall_dir = vec3(0.0f, 0.0f, -1.0f); /* (never faced anywhere yet) */
    b->lie_offset = 0.0f;
    b->face_down  = false;
}

void Body_stretch(Body *b) {
    b->fidget = FIDGET_STRETCH;
    b->fidget_side = 0;
    b->fidget_len = b->fidget_time = FIDGET_LENGTH[FIDGET_STRETCH] * 1.3f;
    b->fidget_fade = 1.0f;
    b->fidget_timer = b->fidget_len + 2.0f;
}

void Body_getUp(Body *b) {
    b->asleep = false;
    if (b->pose != POSE_UP) b->down_time = 0.0f; /* still going down: up again right after */
}

bool Body_isDown(const Body *b) {
    return b->pose != POSE_UP;
}

void Body_burp(Body *b) {
    if (b->burp <= 0.0f && b->pose == POSE_UP) b->burp = BURP_TIME;
}

void Body_vomit(Body *b) {
    if (b->vomit <= 0.0f && b->pose == POSE_UP) b->vomit = VOMIT_TIME;
}

bool Body_takeVomit(Body *b) {
    bool splash = b->vomit_splash;
    b->vomit_splash = false;
    return splash;
}

bool Body_isBusy(const Body *b) {
    return b->pose != POSE_UP || b->vomit > 0.0f || b->pee > 0.0f;
}

void Body_pee(Body *b, float seconds) {
    if (b->pee <= 0.0f && b->pose == POSE_UP) b->pee = b->pee_total = fmaxf(seconds, 1.0f);
}

bool Body_takePee(Body *b) {
    bool done = b->pee_done;
    b->pee_done = false;
    return done;
}

Vec3 Body_takeShift(Body *b) {
    Vec3 shift = b->shift;
    b->shift = vec3_zero();
    return shift;
}

/* Advances falling / lying / getting up. Returns false once standing again. */
static bool updateDown(Body *b, const BodyFrame *f, float s) {
    if (b->pose == POSE_UP) return false;
    b->pose_time += f->dt;
    b->velocity = vec3_zero();
    for (int i = 0; i < 2; i++) b->feet[i].stepping = false;

    /* Feel the ground under the body, so it lies along the bumps */
    Vec3 hips = vec3_add(f->position, vec3_scale(b->fall_dir, b->lie_offset * s));
    for (int k = 0; k < LIE_SAMPLES; k++) {
        Vec3 p = vec3_add(hips, vec3_scale(b->fall_dir, ((float)k - 4.0f) * LIE_STEP));
        b->lie_ground[k] = f->ground(f->ground_ctx, p.x, p.z, f->position.y + 1.0f) - f->position.y;
    }

    switch (b->pose) {
    case POSE_FALLING:
        b->stumble = STUMBLE_TIME * fmaxf(0.0f, 1.0f - b->pose_time / b->fall_time); /* arms out */
        if (b->pose_time >= b->fall_time) { b->pose = POSE_DOWN; b->pose_time = 0.0f; b->stumble = 0.0f; }
        break;
    case POSE_LYING_DOWN:
        if (b->pose_time >= LIE_DOWN_TIME) { b->pose = POSE_DOWN; b->pose_time = 0.0f; }
        break;
    case POSE_DOWN:
        if (b->roll_over != 0.0f) { /* over once, face down onto the back, moving sideways onto it like a log */
            float k = fminf(1.0f, b->pose_time / ROLL_OVER_TIME);
            b->rolling = true;
            b->roll = b->roll_over * PI_F * smoothstepf(0.0f, 1.0f, k);
            b->roll_side = b->roll * ROLL_RADIUS;
            if (k >= 1.0f) { /* on the back now: lying where the roll ended */
                b->shift = vec3_add(b->shift, vec3_scale(vec3_normalize(vec3_cross(b->fall_dir, UP)), b->roll_side * s));
                b->face_down = false;
                b->rolling = false;
                b->roll = b->roll_side = b->roll_over = 0.0f;
                b->down_time = 0.35f;
            }
            break;
        }
        if (b->rolling && b->pose_time < ROLL_TIME) { /* side to side, the whole body turning about its length */
            b->roll = ROLL_REACH * sinf(b->pose_time * ROLL_PACE);
            b->roll_side = b->roll * ROLL_RADIUS;
            break;
        }
        if (b->rolling) b->roll = b->roll_side = 0.0f;
        /* Wanting to move on gets you up sooner */
        if (b->down_time >= 0.0f && (b->down_time -= f->dt * (1.0f + 2.5f * b->urge)) <= 0.0f) {
            /* Get up where the hips lie */
            b->pose = POSE_GETTING_UP;
            b->pose_time = 0.0f;
            b->shift = vec3_scale(b->fall_dir, b->lie_offset * s);
            if (b->rolling) /* up where the roll ended */
                b->shift = vec3_add(b->shift, vec3_scale(vec3_normalize(vec3_cross(b->fall_dir, UP)), b->roll_side * s));
            b->lie_offset = 0.0f;
            b->replant = true;
            /* Drunk or clumsy: getting up may go wrong */
            float u = b->unsteadiness, clumsy = 0.5f + 0.5f * b->quirk[3];  /* 0..1 */
            float fail = clampf((u - 0.55f) * 0.6f, 0.0f, 0.4f);
            float wobble = clampf(0.9f * u + 0.12f * clumsy, 0.0f, 0.9f);
            float r = random01(b);
            b->trouble = r < fail ? 2 : (r < fail + (1.0f - fail) * wobble ? 1 : 0);
        }
        break;
    case POSE_GETTING_UP:
        b->pose_time += f->dt * 0.8f * b->urge; /* quicker when moving on */
        if (b->trouble == 2 && b->pose_time >= 0.55f * GET_UP_TIME) {
            /* Halfway up, the legs give way: back down for a moment */
            float side = random01(b) < 0.5f ? -1.0f : 1.0f;
            Vec3 dir = vec3_add(vec3_scale(rightOf(b->last_facing), side), vec3_scale(b->last_facing, -0.4f));
            b->pose = POSE_FALLING;
            b->pose_time = 0.0f;
            b->fall_dir = vec3_normalize(dir);
            b->lie_offset = FALL_REACH * 0.6f;
            b->face_down = false;
            b->from_knees = true;
            b->fall_style = FALL_PLANK;
            b->fall_twist = 0.0f;
            b->fall_time = FALL_TIME;
            chooseLanding(b, 3.2f);
            b->down_time = 0.8f + 0.8f * random01(b);
            b->trouble = 0;
            break;
        }
        if (b->pose_time >= GET_UP_TIME) {
            b->pose = POSE_UP;
            b->from_knees = false;
            b->rolling = false;
            b->roll = b->roll_side = 0.0f;
            if (b->trouble == 1) {
                /* Up, but not steady yet: a few catching steps */
                float side = random01(b) < 0.5f ? -1.0f : 1.0f;
                Vec3 lurch = vec3_add(vec3_scale(rightOf(b->last_facing), side), vec3_scale(b->last_facing, 0.5f));
                Body_stagger(b, vec3_scale(vec3_normalize(lurch), 0.8f + 1.4f * b->unsteadiness));
            }
            b->trouble = 0;
        }
        break;
    case POSE_UP:
        break;
    }
    return true;
}

/* Room to stand here: nothing under the heel, and room for the toes (which the foot rolls onto). */
static bool spotFree(const BodyFrame *f, Vec3 spot, float s) {
    Vec3 toes = vec3_add(spot, vec3_scale(f->facing, 0.13f * s));
    return f->clutter(f->ground_ctx, spot.x, spot.z, 0.12f * s, f->position.y) <= 0.0f && f->clutter(f->ground_ctx, toes.x, toes.z, 0.08f * s, f->position.y) <= 0.0f;
}

/* The highest thing lying along the way from a to b, a foot's width either side (0 = nothing).
   (The first bit is skipped: whatever the foot is just leaving.) */
static float clutterAlong(const BodyFrame *f, Vec3 a, Vec3 b, float r, float skip) {
    float len = vec3_length(flat(vec3_sub(b, a))), most = 0.0f;
    int n = (int)(len / r) + 1;
    if (n > 8) n = 8;
    for (int k = 0; k <= n; k++) {
        float t = (float)k / (float)n;
        if (t * len < skip) continue;
        Vec3 p = vec3_lerp(a, b, t);
        most = fmaxf(most, f->clutter(f->ground_ctx, p.x, p.z, r, f->position.y));
    }
    return most;
}

/* How deep a leg standing on this ankle would reach into what lies about (0:
   clear): the sole (heel to toes), the shin up to the knee (bent forward, as
   the pose bends it) and the thigh up to the hip, a limb's thickness round
   them. lead: the body moved on this far; hip_y: the hips' height. */
static float legInto(const BodyFrame *f, Vec3 ankle, Vec3 dir, int side, Vec3 lead, float hip_y, float s) {
    Vec3 hip = vec3_add(vec3_add(f->position, lead), vec3_add(vec3(0.0f, hip_y, 0.0f),
                                                             vec3_scale(rightOf(f->facing), (side == 0 ? -HIP_X : HIP_X) * s)));
    /* the knee: two bones from ankle to hip, bending forward */
    Vec3 line = vec3_sub(hip, ankle);
    float d = fmaxf(vec3_length(line), 1e-4f), shin = SHIN * s, thigh = THIGH * s;
    line = vec3_scale(line, 1.0f / d);
    Vec3 knee;
    if (d >= shin + thigh) {
        knee = vec3_add(ankle, vec3_scale(line, shin));
    } else {
        float a = (shin * shin - thigh * thigh + d * d) / (2.0f * d);
        float h = sqrtf(fmaxf(shin * shin - a * a, 0.0f));
        Vec3 fwd = vec3_sub(f->facing, vec3_scale(line, vec3_dot(f->facing, line)));
        fwd = vec3_length(fwd) > 1e-3f ? vec3_normalize(fwd) : UP;
        knee = vec3_add(ankle, vec3_add(vec3_scale(line, a), vec3_scale(fwd, h)));
    }
    const float sole = ankle.y - ANKLE_Y * s;
    float most = 0.0f;
    for (int k = 0; k < 9; k++) {
        Vec3 p;
        float r, bottom;
        if (k < 3) { /* heel, ankle, toes */
            p = vec3_add(ankle, vec3_scale(dir, (k == 0 ? -0.05f : k == 1 ? 0.05f : FOOT_LENGTH) * s));
            r = 0.05f * s;
            bottom = sole;
        } else {     /* up the shin, and the lower thigh */
            p = k < 7 ? vec3_lerp(ankle, knee, 0.25f * (float)(k - 2)) : vec3_lerp(knee, hip, 0.2f * (float)(k - 6));
            r = LIMB_R * s;
            bottom = p.y - LIMB_R * s;
        }
        float q = f->clutter(f->ground_ctx, p.x, p.z, r, bottom - 0.05f); /* (whatever reaches above it here) */
        if (q > 0.0f) most = fmaxf(most, q - 0.05f);
    }
    return most;
}

/* Where a stepping foot aims, carried along as the body moves and turns; but
   a new idea of where to stand (another stance, done standing about) only
   glides over, never jumps (the foot would flick there and back). */
static Vec3 gliding(Foot *foot, Vec3 aim, const BodyFrame *f, float speed, float s, float dt) {
    Vec3 right = rightOf(f->facing);
    Vec3 rel = flat(vec3_sub(aim, f->position));
    Vec3 local = vec3(vec3_dot(rel, right), 0.0f, vec3_dot(rel, f->facing));
    Vec3 change = vec3_sub(local, foot->aim_local);
    float most = (AIM_GLIDE + 1.5f * speed) * s * dt, len = vec3_length(change);
    if (len > most) local = vec3_add(foot->aim_local, vec3_scale(change, most / len));
    foot->aim_local = local;
    Vec3 out = vec3_add(f->position, vec3_add(vec3_scale(right, local.x), vec3_scale(f->facing, local.z)));
    out.y = aim.y;
    return out;
}

/*
 * Drunk: a step may go wrong. Uneven ground and slopes make it more likely.
 * The foot lands off target, the body lurches and the next step is a quick
 * catch. Very drunk, the catch may fail and down you go.
 */
static void maybeStumble(Body *b, const BodyFrame *f, Foot *foot, float speed, float s) {
    float u = b->unsteadiness;
    if (u < 0.05f || b->stumble > 0.0f) return;

    Vec3 spot = foot->step_to;
    Vec3 ahead = vec3_add(spot, vec3_scale(f->facing, 0.3f * s));
    float ground_here  = spot.y - ANKLE_Y * s;
    float ground_ahead = f->ground(f->ground_ctx, ahead.x, ahead.z, spot.y + STEP_UP * s);
    float rough = fabsf(spot.y - foot->ankle.y) / s;          /* step up or down */
    float slope = fabsf(ground_ahead - ground_here) / (0.3f * s);
    /* Tipsy is mostly fine; really drunk is where it gets wobbly */
    float chance = u * u * (0.03f + 1.2f * rough + 0.5f * slope) * (speed > 0.4f ? 1.0f : 0.25f);
    if (random01(b) >= chance) return;

    /* The foot that goes wrong takes the body its way */
    float side = foot == &b->feet[0] ? -1.0f : 1.0f;
    Vec3 right = rightOf(f->facing);
    Vec3 miss = vec3_add(vec3_scale(right, side * (0.12f + 0.2f * u) * s),
                         vec3_scale(f->facing, (random01(b) - 0.4f) * 0.25f * s));
    spot = vec3_add(spot, miss);
    spot.y = groundUnder(f, spot.x, spot.z, s) + ANKLE_Y * s;
    foot->step_to = spot;

    b->stumble = STUMBLE_TIME;
    b->landing = CATCH_STEP_TIME;
    b->stumble_push = vec3_add(vec3_scale(right, side * (0.8f + 1.2f * u)),
                               vec3_scale(f->facing, 0.4f * u));

    if (u > 0.6f && random01(b) < (u - 0.6f) * 0.6f) {
        if (random01(b) < 0.4f) /* reeling backwards a few steps first */
            fallLater(b, vec3_add(vec3_scale(f->facing, -1.0f), vec3_scale(right, side * 0.4f)), 0.5f + 0.6f * random01(b));
        else
            Body_fall(b, vec3_add(vec3_scale(right, side), vec3_scale(f->facing, 0.6f)));
    }
}

/* Head glances, blinking, burps, idle weight shifts. */
static void updateExpression(Body *b, const BodyFrame *f, float speed) {
    const float dt = f->dt;
    b->shown.pride  = ease(b->shown.pride, b->mood.pride, EASE_MOOD, dt);
    b->shown.energy = ease(b->shown.energy, b->mood.energy, EASE_MOOD, dt);
    b->shown.nerves = ease(b->shown.nerves, b->mood.nerves, EASE_MOOD, dt);
    b->chin = ease(b->chin, b->chin_goal, EASE_POSE, dt);
    b->disgust = fmaxf(0.0f, b->disgust - 0.5f * dt);
    b->burp = fmaxf(0.0f, b->burp - dt);
    if (b->pee > 0.0f && (b->pee -= dt * (1.0f + 0.6f * b->pee_power)) <= 0.0f) { /* pushing: done sooner */
        b->pee = 0.0f;
        b->pee_done = true;
    }
    if (b->pose != POSE_UP) b->pee = 0.0f;
    if (b->vomit > 0.0f) {
        float before = VOMIT_TIME - b->vomit;
        b->vomit = fmaxf(0.0f, b->vomit - dt);
        if (before < VOMIT_SPLASH && VOMIT_TIME - b->vomit >= VOMIT_SPLASH) b->vomit_splash = true;
        if (b->pose != POSE_UP) b->vomit = 0.0f;
    }

    /* Turning: the head leads */
    Vec3 last = vec3_length(b->last_facing) > 0.5f ? b->last_facing : f->facing;
    float turned = atan2f(vec3_dot(f->facing, rightOf(last)), vec3_dot(f->facing, last));
    b->turn_rate = ease(b->turn_rate, dt > 0.0f ? turned / dt : 0.0f, 8.0f, dt);
    b->last_facing = f->facing;

    /* Looking around now and then, more often when nervous */
    if ((b->glance_timer -= dt) <= 0.0f) {
        bool ahead = random01(b) < 0.45f;
        float range = speed > 1.5f ? 0.3f : 0.75f;
        b->glance_yaw   = ahead ? 0.0f : (random01(b) * 2.0f - 1.0f) * range;
        b->glance_pitch = ahead ? 0.0f : (random01(b) * 0.4f - 0.25f) * range;
        b->glance_timer = (1.2f + 3.0f * random01(b)) * (1.0f - 0.6f * b->shown.nerves);
    }

    /* Blinking */
    b->blink = fmaxf(0.0f, b->blink - dt);
    if ((b->blink_timer -= dt) <= 0.0f) {
        b->blink = BLINK_TIME;
        b->blink_timer = (2.0f + 4.0f * random01(b)) * (1.0f - 0.5f * b->shown.nerves);
    }

    /* Standing around: weight moves from one leg to the other */
    if ((b->weight_timer -= dt) <= 0.0f) {
        b->weight_goal = random01(b) < 0.3f ? 0.0f : (random01(b) < 0.5f ? -1.0f : 1.0f);
        b->weight_timer = (4.0f + 6.0f * random01(b)) * (1.0f - 0.6f * b->shown.nerves);
    }
    float idle = 1.0f - smoothstepf(0.1f, 0.5f, speed);
    b->weight_shift = ease(b->weight_shift, b->weight_goal * idle, 1.2f, dt);

    /* Now and then one hand does something: on the hip, scratching the head,
       at the chin, in the pocket. Moving or busy hands end it. */
    b->gesture_time = fmaxf(0.0f, b->gesture_time - dt);
    bool talking = b->gesture >= GESTURE_WAVE; /* (said in passing: walking on, it goes on) */
    if (speed > (talking ? 2.5f : 0.3f) || b->pose != POSE_UP || b->hand_active[b->gesture_side]) { /* fades out from where it is */
        if ((b->gesture_fade -= dt / GESTURE_FADE) <= 0.0f) b->gesture_time = 0.0f;
    }
    if ((b->gesture_timer -= dt) <= 0.0f && b->gesture_time <= 0.0f) {
        b->gesture_timer = (5.0f + 9.0f * random01(b)) * (1.0f - 0.5f * b->shown.nerves);
        int side = random01(b) < 0.5f + 0.3f * b->quirk[0] ? 1 : 0;
        if (speed < 0.15f && b->pose == POSE_UP && !b->hand_active[side]) {
            /* which one: how they feel, and their own habits (each person has two they make most) */
            const float nerves = b->shown.nerves, tired = fmaxf(-b->shown.energy, 0.0f), lively = fmaxf(b->shown.energy, 0.0f);
            const float proud = proudness(b), shy = shyness(b);
            float weight[GESTURE_COUNT] = {
                [GESTURE_HIP]    = 0.6f + 1.5f * proud,
                [GESTURE_HEAD]   = 0.6f + 0.8f * nerves,
                [GESTURE_CHIN]   = 0.6f + 0.6f * nerves,
                [GESTURE_POCKET] = 0.8f + 0.6f * shy,
                [GESTURE_NECK]   = 0.3f + 1.2f * tired + 0.6f * shy,
                [GESTURE_NOSE]   = 0.3f + 1.2f * nerves,
                [GESTURE_EAR]    = 0.2f + 1.0f * nerves + 0.5f * shy,
                [GESTURE_HAIR]   = 0.3f + 1.0f * proud + 0.4f * lively,
                [GESTURE_THUMB]  = 0.3f + 1.0f * proud,
                [GESTURE_CHEST]  = 0.2f + 0.8f * shy,
                [GESTURE_SHADE]  = 0.2f + 0.4f * lively,
            };
            weight[b->habit[0]] *= 3.0f;
            weight[b->habit[1]] *= 2.0f;
            float total = 0.0f;
            for (int k = 1; k < GESTURE_COUNT; k++) total += fmaxf(weight[k], 0.0f);
            float r = random01(b) * total;
            int g = GESTURE_POCKET;
            for (int k = 1; k < GESTURE_COUNT; k++) {
                r -= fmaxf(weight[k], 0.0f);
                if (r <= 0.0f) { g = k; break; }
            }
            b->gesture = g;
            b->gesture_side = side;
            b->gesture_len = b->gesture_time = 2.5f + 3.0f * random01(b);
            b->gesture_fade = 1.0f;
        }
    }

    /* The whole body fidgets now and then: shakes a leg loose, rocks on the
       toes, stretches, rolls the shoulders, folds the arms. Who you are
       decides what (nervous: tapping, shaking; tired: stretching; proud:
       arms folded). Walking, working or sitting ends it. */
    bool at_ease = idle > 0.9f && b->pose == POSE_UP && b->sit < 0.05f && b->pee <= 0.0f && b->vomit <= 0.0f &&
                   !b->hand_active[0] && !b->hand_active[1] && b->crouch_goal < 0.1f && b->stumble <= 0.0f;
    b->fidget_time = fmaxf(0.0f, b->fidget_time - dt);
    if (!at_ease && (b->fidget_fade -= dt / FIDGET_FADE) <= 0.0f) b->fidget_time = 0.0f; /* (fades out from where it is) */
    if ((b->fidget_timer -= dt) <= 0.0f && b->fidget_time <= 0.0f) {
        /* (out of breath: soon after stopping, one after the other - catching it) */
        const float puffed = smoothstepf(0.35f, 0.75f, b->exertion);
        b->fidget_timer = (3.0f + 8.0f * random01(b)) * (1.0f - 0.45f * b->shown.nerves) * (1.0f - 0.75f * puffed);
        if (at_ease && b->gesture_time <= 0.0f) {
            const float nerves = b->shown.nerves, tired = fmaxf(-b->shown.energy, 0.0f), proud = proudness(b);
            const float fit = b->fitness;
            const float weight[FIDGET_COUNT] = {
                [FIDGET_SHAKE_LEG]  = 0.8f + 1.5f * nerves,
                [FIDGET_ROCK]       = 0.7f + 0.8f * nerves - 0.4f * tired,
                [FIDGET_STRETCH]    = 0.4f + 1.6f * tired + 1.2f * puffed * fit,
                [FIDGET_SHOULDERS]  = 0.6f + 0.3f * puffed,
                [FIDGET_NECK]       = 0.6f + 0.8f * tired,
                [FIDGET_CROSS_ARMS] = 0.5f + 1.5f * proud - 0.3f * shyness(b),
                [FIDGET_TAP_FOOT]   = 0.3f + 2.0f * nerves,
                [FIDGET_RUB_HANDS]  = 0.2f + 1.2f * nerves,
                [FIDGET_HEAD_BEHIND] = 0.2f + 0.6f * proud + 0.5f * fmaxf(b->shown.energy, 0.0f) - 0.3f * shyness(b),
                [FIDGET_AKIMBO]     = 0.3f + 1.4f * proud - 0.3f * shyness(b) + 2.0f * puffed,
                /* catching breath, by who they are: the unfit bent over, the sporty hands on the head */
                [FIDGET_KNEES]      = 4.0f * puffed * (1.15f - fit) - 1.0f * proud,
                [FIDGET_ON_HEAD]    = 3.5f * puffed * (0.2f + fit),
            };
            float total = 0.0f;
            for (int k = 1; k < FIDGET_COUNT; k++) total += fmaxf(weight[k], 0.0f);
            float r = random01(b) * total;
            int pick = FIDGET_SHAKE_LEG;
            for (int k = 1; k < FIDGET_COUNT; k++) {
                r -= fmaxf(weight[k], 0.0f);
                if (r <= 0.0f) { pick = k; break; }
            }
            b->fidget = pick;
            b->fidget_side = random01(b) < 0.5f ? 0 : 1;
            b->fidget_len = b->fidget_time = FIDGET_LENGTH[pick] * (0.8f + 0.4f * random01(b));
            b->fidget_fade = 1.0f;
        }
    }

    /* Standing about, the feet settle into another stance now and then: one
       a little ahead, wider apart, back together (each change is a step) */
    if (idle < 0.5f) {
        b->stance[0] = b->stance[1] = vec3_zero();
        b->stance_timer = 4.0f + 6.0f * random01(b);
    } else if (at_ease && (b->stance_timer -= dt) <= 0.0f) { /* (not while busy: hands on the knees, ...) */
        b->stance_timer = (5.0f + 10.0f * random01(b)) * (1.0f - 0.4f * b->shown.nerves);
        int side = random01(b) < 0.5f ? 0 : 1;
        float r = random01(b);
        Vec3 old = b->stance[side];
        if (r < 0.35f)      b->stance[side] = vec3(0.0f, 0.0f, STANCE_STEP * (random01(b) < 0.7f ? 1.0f : -0.8f)); /* one foot ahead (or back) */
        else if (r < 0.55f) b->stance[side] = vec3((side ? 1.0f : -1.0f) * STANCE_STEP * 0.9f, 0.0f, 0.0f);      /* wider */
        else                b->stance[side] = vec3_zero();                                                       /* back together */
        if (vec3_length(vec3_sub(old, b->stance[side])) < 0.05f) b->stance[side] = vec3(0.0f, 0.0f, STANCE_STEP);
        b->stance[1 - side] = vec3_scale(b->stance[1 - side], 0.5f);
    }
}

void Body_talkGesture(Body *b, int kind) {
    int side = !b->hand_active[1] ? 1 : !b->hand_active[0] ? 0 : -1;
    if (side < 0 || b->pose != POSE_UP) return;
    b->gesture = kind == TALK_WAVE ? GESTURE_WAVE : kind == TALK_SHRUG ? GESTURE_SHRUG : GESTURE_POINT;
    b->gesture_side = side;
    b->gesture_len = b->gesture_time = kind == TALK_WAVE ? 1.8f : 1.4f;
    b->gesture_fade = 1.0f;
    b->gesture_timer = b->gesture_len + 4.0f;
}

void Body_think(Body *b, float seconds) {
    int side = !b->hand_active[1] ? 1 : !b->hand_active[0] ? 0 : -1;
    if (side < 0 || b->pose != POSE_UP || seconds < 0.5f) return;
    b->gesture = random01(b) < 0.65f ? GESTURE_CHIN : GESTURE_HEAD;
    b->gesture_side = side;
    b->gesture_len = b->gesture_time = seconds;
    b->gesture_fade = 1.0f;
    b->gesture_timer = seconds + 3.0f;
}

/* Leaving the ground: which foot pushes off last, and (running) which one leads. */
static void takeOff(Body *b, const BodyFrame *f, float speed) {
    b->jumping = true;
    b->lead_foot = -1;
    Vec3 fwd = speed > 0.6f ? vec3_normalize(flat(b->velocity)) : f->facing;
    float ahead0 = vec3_dot(vec3_sub(b->feet[0].ankle, f->position), fwd);
    float ahead1 = vec3_dot(vec3_sub(b->feet[1].ankle, f->position), fwd);
    if (speed > 0.8f) {
        b->push_foot = ahead0 < ahead1 ? 0 : 1; /* the rear foot pushes off ... */
        b->lead_foot = 1 - b->push_foot;        /* ... the front one goes for the landing */
        b->push_time = PUSH_OFF_TIME * 1.2f;
    } else {
        /* Standing: nearly always one foot a little before the other */
        float r = random01(b);
        b->push_foot = r < 0.12f ? -1 : (r < 0.56f + 0.2f * b->quirk[0] ? 1 : 0);
        b->push_time = PUSH_OFF_TIME * (0.6f + 0.6f * random01(b));
    }
    for (int i = 0; i < 2; i++) b->feet[i].stepping = false;
}

/* Feet off the ground: tucked under the body; running, the leading leg
   reaches forward and the trailing one hangs back, heel up. */
static void inTheAir(Body *b, const BodyFrame *f, float s, float speed) {
    const float dt = f->dt;
    Vec3 fwd = speed > 0.6f ? vec3_normalize(flat(b->velocity)) : f->facing;
    bool rising = b->vy > 0.0f;
    /* Dropping from high up: knees pulled up to the belly, then (timed) the legs
       reach down for the ground */
    float tuck = 0.0f, reach = 0.0f;
    if (b->vy < -2.0f && f->ground) {
        float h = f->position.y - f->ground(f->ground_ctx, f->position.x, f->position.z, f->position.y + 0.1f);
        float v = -b->vy, t = (-v + sqrtf(v * v + 2.0f * 9.81f * fmaxf(h, 0.0f))) / 9.81f; /* (till the feet are down) */
        float high = smoothstepf(0.5f * s, 1.2f * s, h + 0.1f * v * v);
        reach = high * (1.0f - smoothstepf(0.12f, 0.28f, t));
        tuck  = high * smoothstepf(2.0f, 4.0f, v) * (1.0f - reach);
    }
    for (int i = 0; i < 2; i++) {
        Foot *foot = &b->feet[i];
        foot->stepping = false;
        if (i == b->push_foot && b->air_time < b->push_time && tuck <= 0.0f) continue; /* still pushing off */
        float sign = i == 0 ? -1.0f : 1.0f;
        float ahead, lift, pitch;
        if (b->lead_foot >= 0) {
            bool lead = i == b->lead_foot;
            ahead = lead ? 0.34f : -0.30f;
            lift  = lead ? (rising ? 0.30f : 0.10f) : 0.26f;
            pitch = lead ? 0.25f : -0.9f;       /* toes up to land on the heel / pointing down behind */
        } else {
            ahead = 0.03f;
            /* the foot that left first tucks up higher, the push-off leg stays longer */
            lift  = (rising ? 0.20f : 0.11f) + (b->push_foot < 0 ? 0.0f : (i == b->push_foot ? -0.04f : 0.04f));
            pitch = -0.35f;
        }
        if (tuck + reach > 0.0f) { /* (knees up, then legs down, feet flat to land) */
            ahead = ahead + (0.12f - ahead) * tuck + (0.05f - ahead) * reach;
            lift  = lift + (0.40f - lift) * tuck - lift * reach;
            pitch = pitch + (-0.15f - pitch) * fmaxf(tuck, reach);
        }
        Vec3 hang = vec3_add(f->position, vec3_add(vec3_scale(rightOf(f->facing), sign * FOOT_X * s), vec3_scale(fwd, ahead * s)));
        hang.y = f->position.y + lift * s;
        /* on a spring, from the way it was moving when it left the ground */
        float omega = reach > 0.3f ? 22.0f : i == b->push_foot ? 10.0f : 14.0f; /* (reaching for the ground: quick) */
        if (!b->airborne) foot->air_v = foot->vel;
        Vec3 was = vec3_sub(foot->toe, foot->ankle); /* (how the foot pointed, before it moves on) */
        Vec3 carried = vec3(b->velocity.x, b->vy, b->velocity.z); /* (it goes along with the body) */
        springStep3(&foot->ankle, &foot->air_v, hang, carried, omega, dt);
        foot->dir = f->facing;
        Vec3 along = vec3_add(vec3_scale(f->facing, cosf(pitch)), vec3_scale(UP, sinf(pitch))); /* (toes the way the body faces) */
        if (vec3_length(was) > 1e-4f) along = vec3_normalize(vec3_lerp(vec3_normalize(was), along, fminf(1.0f, 10.0f * dt)));
        foot->toe = vec3_add(foot->ankle, vec3_scale(along, FOOT_LENGTH * s));
        if (vec3_length(flat(along)) > 0.05f) foot->dir = vec3_normalize(flat(along)); /* (lands pointing that way) */
    }
    b->airborne = true;
}

/* On something narrow (a log): the arms go out and the body teeters from side
   to side - a wobble that grows with clumsiness, drink and hurry and that the
   sporty keep small. Past the edge of it: a step off sideways. */
#define TEETER_OMEGA  3.2f  /* rad/s: how quickly a wobble swings back */
#define TEETER_DAMP   0.35f /* (some is caught, the rest swings on) */
#define TEETER_KICK   40.0f  /* how hard the wobbles come */

static void updateBalance(Body *b, const BodyFrame *f, float speed, float dt) {
    b->balance += (b->balance_goal - b->balance) * fminf(1.0f, (b->balance_goal > b->balance ? 4.0f : 2.0f) * dt);
    b->dance += (b->dance_goal - b->dance) * fminf(1.0f, 2.0f * dt);
    if (b->dance < 0.002f) b->dance = 0.0f;
    if (b->balance < 0.02f || b->airborne || b->on_ladder || dt <= 0.0f) {
        b->teeter *= fmaxf(0.0f, 1.0f - 3.0f * dt);
        b->teeter_v = 0.0f;
        return;
    }
    float shaky = (0.25f + 0.6f * Body_clumsiness(b) + 1.6f * b->unsteadiness + 0.35f * fminf(speed, 2.0f) + 0.4f * b->shown.nerves)
                * (1.25f - 0.75f * b->fitness) * b->balance;
    float kick = (random01(b) * 2.0f - 1.0f) * TEETER_KICK * shaky;
    b->teeter_v += (kick - TEETER_OMEGA * TEETER_OMEGA * b->teeter - 2.0f * TEETER_DAMP * TEETER_OMEGA * b->teeter_v) * dt;
    b->teeter += b->teeter_v * dt;
    if (fabsf(b->teeter) > 1.0f && b->stumble <= 0.0f) { /* too far over: a step off, that side */
        float side = b->teeter > 0.0f ? 1.0f : -1.0f;
        Body_stagger(b, vec3_add(vec3_scale(rightOf(f->facing), side * 1.3f), vec3_scale(f->facing, 0.2f)));
        b->teeter = b->teeter_v = 0.0f;
        b->stepped_off = true;
    }
}

void Body_update(Body *b, const BodyFrame *f) {
    const float s = f->height / REF_HEIGHT;
    const float dt = f->dt;
    b->clock += dt;
    b->clutter = f->clutter; /* (the pose keeps the legs clear of it) */
    b->clutter_ctx = f->ground_ctx;
    b->ground = f->ground;
    b->push_out = f->push_out;
    b->at = f->position;
    b->at_scale = s;
    for (int i = 0; i < 2; i++) { /* how the feet moved last frame */
        Foot *foot = &b->feet[i];
        foot->vel = dt > 0.0f ? vec3_scale(vec3_sub(foot->ankle, foot->prev), 1.0f / dt) : vec3_zero();
        if (vec3_length(foot->vel) > 15.0f) foot->vel = vec3_zero(); /* (put somewhere else) */
        foot->prev = foot->ankle;
    }

    /* Velocity from actual motion, whatever caused it (walking, pushing, being pushed) */
    if (!b->ready) {
        b->ready = true;
        /* Everybody gets their own dice, and their own little lopsided ways */
        b->rng = 0x9E3779B9u ^ (unsigned)(int)(f->position.x * 7919.0f) ^ ((unsigned)(int)(f->position.z * 104729.0f) << 7);
        if (b->rng == 0) b->rng = 1;
        for (int k = 0; k < 4; k++) b->quirk[k] = random01(b) * 2.0f - 1.0f;
        for (int k = 0; k < 4; k++) { /* (from the quirks, not the dice: everything else stays as it was) */
            float h = sinf(b->quirk[k] * 127.1f + b->quirk[(k + 1) % 4] * 311.7f + (float)k * 74.7f) * 43758.5453f;
            b->style[k] = (h - floorf(h)) * 2.0f - 1.0f;
        }
        {   float h = sinf(b->quirk[0] * 91.3f + b->quirk[2] * 47.1f + 13.7f) * 43758.5453f;
            b->stride = (h - floorf(h)) * 2.0f - 1.0f; } /* (long strides, or short quick ones: the same speed) */
        b->habit[0] = (unsigned char)(1 + (int)(random01(b) * (float)(GESTURE_COUNT - 1)) % (GESTURE_COUNT - 1));
        b->habit[1] = (unsigned char)(1 + ((int)b->habit[0] + (int)(random01(b) * (float)(GESTURE_COUNT - 2))) % (GESTURE_COUNT - 1));
        b->gesture_timer = 3.0f + 6.0f * random01(b);
        b->last_position = f->position;
        b->last_facing = f->facing;
        b->velocity = vec3_zero();
        b->step_length = stepLength(0.0f, s);
        b->pelvis_y = PELVIS_Y;
        b->last_y = f->position.y;
        b->push_foot = b->lead_foot = -1;
        plantAll(b, f, s);
    }
    if (b->replant) {
        b->replant = false;
        b->last_position = f->position;
        plantAll(b, f, s);
    }
    Vec3 moved = flat(vec3_sub(f->position, b->last_position));
    b->last_position = f->position;
    Vec3 v = dt > 0.0f ? vec3_scale(moved, 1.0f / dt) : vec3_zero();
    if (vec3_length(v) > TELEPORT_SPEED) {
        b->velocity = vec3_zero();
        plantAll(b, f, s);
    }
    b->velocity = vec3_lerp(b->velocity, v, fminf(1.0f, EASE_VELOCITY * dt));
    /* The body itself carries some weight: when the character jitters (pushed
       about, wedged against something) the hips don't shake with it */
    if (!b->root_ok || vec3_length(v) > TELEPORT_SPEED) {
        b->root_vis = flat(f->position);
        b->root_vel = flat(b->velocity);
        b->root_ok = true;
    } else if (dt > 0.0f) {
        springStep3(&b->root_vis, &b->root_vel, flat(f->position), flat(b->velocity), ROOT_OMEGA, dt);
        Vec3 off = vec3_sub(b->root_vis, flat(f->position));
        float far = vec3_length(off), most = ROOT_LAG * s;
        if (far > most) b->root_vis = vec3_add(flat(f->position), vec3_scale(off, most / far));
    }
    float speed = vec3_length(b->velocity);
    updateExpression(b, f, speed);
    updateSmoking(b, speed, dt);
    if (updateDown(b, f, s)) return;
    if (b->shoulder_roll > 0.0f) { /* over the shoulder: a set move, the feet go where the body is */
        b->shoulder_roll += dt / SHOULDER_ROLL_TIME;
        Vec3 r = rightOf(f->facing);
        for (int i = 0; i < 2; i++) { /* (the feet come along with the body: under it when up again) */
            Foot *foot = &b->feet[i];
            foot->stepping = false;
            foot->ankle = vec3_add(f->position, vec3_add(vec3_scale(r, (i == 0 ? -0.1f : 0.1f) * s), vec3(0.0f, 0.075f * s, 0.0f)));
            foot->dir = f->facing;
            foot->toe = vec3_add(foot->ankle, vec3_add(vec3_scale(f->facing, 0.14f * s), vec3(0.0f, -0.05f * s, 0.0f)));
        }
        if (b->shoulder_roll < 1.0f) return;
        b->shoulder_roll = 0.0f;
        b->replant = true; /* (up again: the feet under you) */
    }
    updateBalance(b, f, speed, dt);

    /* Side steps and backward steps are shorter than steps forward; shy people take small steps */
    float sideways = speed > 0.05f ? fabsf(vec3_dot(b->velocity, rightOf(f->facing))) / speed : 0.0f;
    float backward = speed > 0.05f ? fmaxf(0.0f, -vec3_dot(b->velocity, f->facing) / speed) : 0.0f;
    b->step_length = stepLength(speed, s) * (1.0f - 0.45f * sideways) * (1.0f - 0.35f * backward)
                   * (1.0f - 0.12f * shyness(b))
                   * (1.0f + 0.14f * b->stride)                                  /* their own: long and slow, or short and quick */
                   * (1.0f + 0.3f * (b->fitness - 0.5f) * runAmount(speed));     /* (sprinting, the sporty bound along) */

    /* Something to step over coming up: the sporty fit their steps to it, so a
       foot comes down at a good take-off spot just before it, no stutter */
    if (f->clutter && speed > 0.6f && b->fitness > PLAN_FIT && backward < 0.3f) {
        Vec3 dir = vec3_scale(flat(b->velocity), 1.0f / speed);
        float edge = -1.0f;
        for (float d = 0.3f * s; d < PLAN_AHEAD * s && edge < 0.0f; d += 0.1f * s) {
            Vec3 p = vec3_add(f->position, vec3_scale(dir, d));
            if (f->clutter(f->ground_ctx, p.x, p.z, 0.1f * s, f->position.y) > 0.04f * s) edge = d;
        }
        if (edge > 0.0f) {
            float take_off = (PLAN_TAKE_OFF + 0.08f * speed) * s;
            float room = edge - take_off - 0.5f * b->step_length; /* (the next foot lands half a step ahead) */
            if (room > 0.0f) {
                float n = fmaxf(1.0f, roundf(room / b->step_length));
                float fit = clampf(room / n, 0.75f * b->step_length, 1.25f * b->step_length);
                b->step_length = lerpf(b->step_length, fit, smoothstepf(PLAN_FIT, PLAN_FIT + 0.4f, b->fitness));
            }
        }
    }

    /* Uphill: shorter steps; downhill: longer ones */
    float grade = 0.0f;
    if (speed > 0.2f) {
        Vec3 ahead = vec3_add(f->position, vec3_scale(flat(b->velocity), 0.5f * s / speed));
        float from_y = f->position.y + STEP_UP * s;
        grade = (f->ground(f->ground_ctx, ahead.x, ahead.z, from_y)
               - f->ground(f->ground_ctx, f->position.x, f->position.z, from_y)) / (0.5f * s);
    }
    b->grade = ease(b->grade, clampf(grade, -1.0f, 1.0f), 5.0f, dt);
    b->step_length *= b->grade > 0.0f ? 1.0f - clampf(b->grade * 1.5f, 0.0f, 0.45f)
                                      : 1.0f + clampf(-b->grade * 0.9f, 0.0f, 0.3f);

    /* Action goals */
    if (b->squat_pee && b->pee > 0.0f) b->crouch_goal = fmaxf(b->crouch_goal, 1.0f); /* squatting to pee */
    if (b->land_squat > 0.0f) { /* landed from high up: into the knees, up again */
        b->crouch_goal = fmaxf(b->crouch_goal, b->land_squat);
        b->land_squat = fmaxf(0.0f, b->land_squat - 1.6f * dt);
    }
    springCritical(&b->crouch, &b->crouch_v, b->crouch_goal, POSE_OMEGA, dt); /* bending down speeds up, slows down */
    b->crouch = clampf(b->crouch, 0.0f, 1.2f);
    b->sit    = ease(b->sit, b->pose == POSE_UP ? b->sit_goal : 0.0f, 2.5f, dt);
    springCritical(&b->lean, &b->lean_v, b->lean_goal, POSE_OMEGA, dt);
    for (int h = 0; h < 2; h++)
    {   /* eases in and out (a spring): a hand never jerks off towards something */
        springCritical(&b->hand_weight[h], &b->hand_wvel[h], b->hand_active[h] ? 1.0f : 0.0f, HAND_WEIGHT_OMEGA, dt);
        b->hand_weight[h] = clampf(b->hand_weight[h], 0.0f, 1.0f);
    }

    /* Jumping: off one foot (rarely both at once); running, the rear foot
       pushes off and the front one reaches ahead to land first */
    float rise = dt > 0.0f ? (f->position.y - b->last_y) / dt : 0.0f;
    if (f->grounded && fabsf(f->position.y - b->last_y) > 0.08f * s && fabsf(f->position.y - b->last_y) < 0.7f * s) {
        /* hopped up (or dropped) onto something in one go: the hips stay where they were and follow */
        b->pelvis_y -= (f->position.y - b->last_y) / s;
        rise = 0.0f;
    }
    b->vy = ease(b->vy, rise, 20.0f, dt);
    b->last_y = f->position.y;
    if (!f->grounded && b->air_time <= 0.0f && rise > TAKE_OFF_SPEED && !b->jumping) takeOff(b, f, speed);
    b->air_time = f->grounded ? 0.0f : b->air_time + dt;
    if (b->air_time > AIR_GRACE || (b->jumping && !f->grounded)) {
        inTheAir(b, f, s, speed);
        return;
    }
    if (b->airborne && f->grounded) {
        /* Landing: both feet reach for the ground quickly, one a moment before the other */
        b->airborne = false;
        b->landing = LANDING_STEP_TIME;
        for (int i = 0; i < 2; i++) {
            b->feet[i].stepping  = true;
            b->feet[i].step      = 0.0f;
            b->feet[i].step_from = b->feet[i].ankle;
            b->feet[i].step_v0   = b->feet[i].vel;   /* (on from how it was moving) */
            if (vec3_length(b->feet[i].step_v0) > 4.0f * s) b->feet[i].step_v0 = vec3_scale(vec3_normalize(b->feet[i].step_v0), 4.0f * s);
            b->feet[i].spot_was  = b->feet[i].step_to = ankleAt(footSpot(b, f, i, s), s);
            b->feet[i].pitch0    = asinf(clampf((b->feet[i].toe.y - b->feet[i].ankle.y) / fmaxf(FOOT_LENGTH * s, 1e-4f), -1.0f, 1.0f));
            b->feet[i].delay     = 0.0f;
            b->feet[i].clear_shift = vec3_zero();
            b->feet[i].over      = 0.0f;
        }
        if (b->lead_foot >= 0) b->feet[1 - b->lead_foot].delay = TRAIL_LAND_DELAY;
        else if (b->push_foot >= 0) b->feet[b->push_foot].delay = 0.04f;
    }
    if (f->grounded) {
        b->jumping = false;
        b->lead_foot = b->push_foot = -1;
    }
    b->landing = fmaxf(0.0f, b->landing - dt);
    b->stumble = fmaxf(0.0f, b->stumble - dt);
    if (b->fall_delay > 0.0f) { /* reeling: keeps going, then down */
        b->stumble = fmaxf(b->stumble, 0.3f);
        if ((b->fall_delay -= dt) <= 0.0f) Body_fall(b, b->fall_pending);
        else if (b->fall_delay < 0.3f && b->fall_delay + dt >= 0.3f)
            b->stumble_push = vec3_add(b->stumble_push, vec3_scale(b->fall_pending, 0.9f));
    }

    /* Swing time: walking ~ one step, running longer (flight phase) */
    /* (setting off, the body is already going: quick first steps keep up with it) */
    const float pace = fmaxf(speed, fminf(vec3_length(v), 2.0f * speed + 1.0f));
    const float swing = b->step_length / fmaxf(pace, 0.01f) * (1.0f + 0.4f * runAmount(speed));
    const float step_time = b->landing > 0.0f ? LANDING_STEP_TIME
                          : clampf(swing, MIN_STEP_TIME, MAX_STEP_TIME);
    const float hip_y = ((b->pelvis_y > 0.3f ? b->pelvis_y : PELVIS_Y) - HIP_DROP) * s; /* (the legs hang from there) */

    /* Feet that are stepping keep aiming at their (moving) spot */
    bool any_stepping = false;
    for (int i = 0; i < 2; i++) {
        Foot *foot = &b->feet[i];
        if (!foot->stepping) continue;
        if (foot->delay > 0.0f) { /* still in the air a moment: stays where it is */
            foot->delay -= dt;
            any_stepping = true;
            continue;
        }
        /* the spot moves with the body; the way round what lies there was chosen when the step began */
        Vec3 spot = uncrossed(footSpot(b, f, i, s), f, i, b->feet[1 - i].ankle, s);
        if (vec3_length(foot->clear_shift) > 1e-4f) {
            spot = vec3_add(spot, foot->clear_shift);
            spot.y = groundUnder(f, spot.x, spot.z, s);
        }
        /* The aim moves with the body, less and less towards the end: the
           foot slows down onto the ground instead of being dragged along */
        Vec3 aim = gliding(foot, ankleAt(spot, s), f, vec3_length(flat(b->velocity)), s, dt);
        float follow = 1.0f - smoothstepf(0.55f, 0.95f, foot->step);
        Vec3 next = vec3_add(foot->step_to, vec3_scale(flat(vec3_sub(aim, foot->spot_was)), follow));
        /* (but not onto something lying there: then it lands where it was going) */
        bool lunge = foot->over > 0.0f && vec3_length(flat(vec3_sub(next, b->feet[1 - i].ankle))) > overReach(b, s) &&
                     vec3_length(flat(vec3_sub(next, b->feet[1 - i].ankle))) > vec3_length(flat(vec3_sub(foot->step_to, b->feet[1 - i].ankle)));
        if (!lunge && (!f->clutter || spotFree(f, next, s) || !spotFree(f, foot->step_to, s)))
            foot->step_to = next;
        else if (foot->step < OVER_LEAP_LATE && vec3_length(flat(vec3_sub(next, foot->step_to))) > OVER_LEFT * s)
            foot->step_to = leapPast(f, foot, next, b->feet[1 - i].stepping ? b->feet[1 - i].step_to : b->feet[1 - i].ankle, overReach(b, s), s);
        foot->spot_was = aim;
        /* (over an edge the ground below the aim jumps: the foot goes there smoothly) */
        float to_y = groundUnder(f, foot->step_to.x, foot->step_to.z, s) + ANKLE_Y * s;
        foot->step_to.y = ease(foot->step_to.y, to_y, foot->step > 0.85f ? 40.0f : 18.0f, dt);
        /* (over something: slower; running: a leap, and a step held back behind the body comes down quick) */
        float late = speed > LEAP_SPEED ? fmaxf(0.0f, -vec3_dot(flat(vec3_sub(foot->step_to, f->position)), f->facing)) / s : 0.0f;
        const float step_was = foot->step;
        foot->step = fminf(1.0f, foot->step + dt * (1.0f + LATE_QUICK * late) / (step_time * (1.0f + OVER_SLOW * (1.0f - runAmount(speed)) * foot->over / s)));

        /* The foot rises early in the swing and reaches forward late. Running:
           the heel kicks up behind first, then the knee drives forward. */
        float run = runAmount(speed);
        float t = smoothstepf(0.0f, 1.0f, powf(foot->step, 1.0f + 0.8f * run)); /* (running: stays behind a while, then comes through) */
        Vec3 pos = vec3_lerp(foot->step_from, foot->step_to, t);
        {   /* a foot already on the move (coming down from a jump) carries on smoothly */
            float u = foot->step;
            pos = vec3_add(pos, vec3_scale(foot->step_v0, u * (1.0f - u) * (1.0f - u) * step_time));
        }
        if (f->clutter && foot->step < 0.9f) {
            /* something turned up in the way (the aim moved on with the body): lift higher, quickly but smoothly */
            float need = clutterAlong(f, pos, foot->step_to, 0.09f * s, 0.0f);
            float want = need > 0.0f ? need + OVER_CLEAR * s : 0.0f;
            if (want > foot->over) foot->over = fminf(want, foot->over + OVER_RISE * s * dt);
        }
        pos.y += fmaxf((STEP_LIFT + 0.3f * run) * s * stepArc(foot->step, 0.75f - 0.35f * run) /* (running: the heel up behind, early) */
                         * clampf(0.4f + speed * 0.3f, 0.4f, 1.2f) * (1.0f + 0.25f * b->shown.energy),
                       foot->over * stepArc(foot->step, 1.0f)); /* a big step over something (no higher than that) */
        foot->swing_run = fmaxf(run, foot->swing_run - SWING_RUN_EASE * dt); /* (slowing down mid-swing: the curve goes over gently) */
        if (foot->swing_run > 0.0f && foot->over <= 0.0f) {
            /* Running: off the toes the heel folds up under the seat, the knee drives through in
               front, the foot reaches out and comes down (a curve that goes with the body) */
            Vec3 side = vec3_scale(rightOf(f->facing), (i == 0 ? -1.0f : 1.0f) * 0.07f * s); /* (running: the feet nearly on one line) */
            Vec3 tuck  = vec3_add(vec3_add(f->position, side), vec3_add(vec3_scale(f->facing, RUN_TUCK_BACK * s), vec3(0.0f, RUN_TUCK_UP * s, 0.0f)));
            Vec3 drive = vec3_add(vec3_add(f->position, side), vec3_add(vec3_scale(f->facing, RUN_DRIVE_AHEAD * s), vec3(0.0f, RUN_DRIVE_UP * s, 0.0f)));
            float u = foot->step, m = 1.0f - u;
            Vec3 curve = vec3_add(vec3_add(vec3_scale(foot->step_from, m * m * m), vec3_scale(tuck, 3.0f * m * m * u)),
                                  vec3_add(vec3_scale(drive, 3.0f * m * u * u), vec3_scale(foot->step_to, u * u * u)));
            pos = vec3_lerp(pos, curve, foot->swing_run);
        }
        if (foot->over > 0.0f) {
            /* Over something the leg swings round a little, out to the side and back in to land,
               while the body's momentum carries it on: the shin clears without climbing so high */
            float out = OVER_SWING_OUT * s * smoothstepf(0.0f, OVER_SWING_FULL * s, foot->over) * stepArc(foot->step, 1.0f);
            pos = vec3_add(pos, vec3_scale(rightOf(f->facing), i == 0 ? -out : out));
        }
        if (f->clutter) {
            /* never into what lies there: lifted over it, and over what it's about to reach (smoothly) */
            Vec3 ahead = vec3_add(pos, vec3_scale(flat(vec3_sub(foot->step_to, foot->step_from)), 0.25f));
            float floor = fmaxf(groundUnder(f, pos.x, pos.z, s), groundUnder(f, ahead.x, ahead.z, s)) + (ANKLE_Y + 0.02f) * s;
            float want = foot->step < 0.97f ? fmaxf(0.0f, floor - pos.y) : 0.0f;
            /* the whole leg too: the shin and the knee come over it, not through it */
            /* (a foot far off is just catching up; standing about, feet just shift, they don't climb anything) */
            bool near = vec3_length(flat(vec3_sub(pos, f->position))) < LEG_NEAR * s && speed > LEG_MOVING;
            float into = foot->step < 0.97f && near ? legInto(f, vec3_add(pos, vec3(0.0f, foot->lift, 0.0f)), foot->dir, i, vec3_zero(), hip_y, s) : 0.0f;
            want = fminf(fmaxf(want, foot->lift + into), LIFT_MOST * s);
            float up = (into > 0.0f ? LEG_RISE : LIFT_RISE) * s * dt;
            foot->lift = want > foot->lift ? fminf(want, foot->lift + up) : fmaxf(want, foot->lift - LIFT_RISE * s * dt);
            /* down onto the ground with the step: never faster than a foot comes down, and the step
               isn't over before the foot is down */
            float cap = LIFT_MOST * s * (1.0f - smoothstepf(LEG_HOLD - 0.05f, 0.97f, foot->step));
            if (foot->lift > cap) foot->lift = fmaxf(cap, foot->lift - LIFT_DROP * s * dt);
            if (foot->lift > 0.03f * s && foot->step > 0.95f) foot->step = 0.95f;
            pos.y += foot->lift;
            /* not clear yet: the foot goes up first and over after (unless it can't get that high) */
            /* (running there's no waiting: the knee drives through, high, and it may scrape) */
            if (near && speed < LEAP_SPEED && foot->step < LEG_HOLD && foot->lift < LIFT_MOST * s - 0.01f &&
                legInto(f, pos, foot->dir, i, vec3_zero(), hip_y, s) > LEG_TOUCH * s) {
                pos.x = foot->ankle.x;
                pos.z = foot->ankle.z;
                foot->step = step_was;
            }
        }
        foot->ankle = pos;
        foot->dir = vec3_normalize(vec3_lerp(foot->dir, footDir(b, f->facing, i), fminf(1.0f, dt * 12.0f)));
        float strike = heelStrike(run) * (b->landing > 0.0f ? 0.3f : 1.0f);
        foot->toe = toeSwinging(f, pos, foot->dir, foot->step, foot->pitch0, strike, s);

        if (foot->step >= 1.0f) {
            foot->stepping = false;
            foot->ankle = foot->step_to;
            b->landed = i + 1;
            b->landed_at = foot->ankle;
            foot->toe = toePlanted(f, foot->ankle, foot->dir, s);
            foot->slap = strike;  /* on the heel: the toes come down next */
            foot->slap_v = 0.0f;
            b->last_step = i;
        } else {
            any_stepping = true;
        }
    }

    crossOver(b, f, s);

    /* Start a new step with the foot that drifted furthest from its spot (or
       that the body turned away from). Walking: one foot at a time.
       Running: the next foot may leave before the other lands. */
    float overlap = 0.45f * runAmount(speed);
    float limit = (speed > 0.2f ? STEP_THRESHOLD : IDLE_THRESHOLD) * s;
    int best = -1;
    float best_need = 0.0f;
    Vec3 best_spot = vec3_zero();
    for (int i = 0; i < 2; i++) b->feet[i].planted = b->feet[i].stepping ? 0.0f : b->feet[i].planted + dt;
    for (int i = 0; i < 2; i++) {
        Foot *foot = &b->feet[i], *other = &b->feet[1 - i];
        if (foot->stepping) continue;
        /* (running, the foot that came down stays down a moment: the leg bends under the body and pushes off) */
        if (foot->planted < STANCE_RUN * runAmount(speed) && foot->into <= LEG_TOUCH * s) continue;
        /* (running, a foot left far behind while the other is still on its way over something: off it goes, a leap) */
        float behind = -vec3_dot(flat(vec3_sub(foot->ankle, f->position)), f->facing);
        bool leap = speed > LEAP_SPEED && ((other->over > 0.0f && behind > LEAP_BEHIND * s) || foot->into > LEG_TOUCH * s);
        if (other->stepping && other->step < 1.0f - overlap && !leap) continue;
        if (any_stepping && !other->stepping) continue;
        /* half over something: the foot already over there stays; the other one comes after it */
        if (b->crossing && !other->stepping && vec3_dot(flat(vec3_sub(foot->ankle, b->cross_to)), b->cross_dir) > -0.12f * s &&
            vec3_dot(flat(vec3_sub(other->ankle, b->cross_to)), b->cross_dir) < -0.25f * s) continue;

        Vec3 spot = ankleAt(uncrossed(footSpot(b, f, i, s), f, i, other->ankle, s), s);
        float drift  = vec3_length(flat(vec3_sub(spot, foot->ankle)));
        float aligned = vec3_dot(foot->dir, footDir(b, f->facing, i));
        bool turned  = aligned < TURN_THRESHOLD || (speed < 0.2f && aligned < 0.95f); /* standing: settle the feet */
        bool climb   = fabsf(spot.y - foot->ankle.y) > 0.15f * s;
        float need   = drift / limit + (turned ? 1.0f : 0.0f) + (climb ? 1.0f : 0.0f);
        if (i != b->last_step) need += 0.05f; /* tie: alternate feet */
        if (foot->into > LEG_TOUCH * s)
            need += 1.5f; /* the body moved on over something: this leg would go through it, up it comes */
        if (need > 1.0f && need > best_need) {
            best = i;
            best_need = need;
            best_spot = spot;
        }
    }
    if (best >= 0) {
        Foot *foot = &b->feet[best];
        foot->stepping  = true;
        foot->step      = 0.0f;
        foot->swing_run = runAmount(speed);
        /* off from where it was drawn (heel up, rolled over the toes): no jump */
        bool shown = b->shown_valid && vec3_length(vec3_sub(b->ankle_shown[best], foot->ankle)) < 0.25f * s;
        foot->step_from = shown ? b->ankle_shown[best] : foot->ankle;
        if (shown) {
            Vec3 d = vec3_sub(b->toe_shown[best], b->ankle_shown[best]);
            foot->pitch0 = asinf(clampf(d.y / fmaxf(vec3_length(d), 1e-4f), -1.0f, 1.0f));
            foot->ankle = b->ankle_shown[best]; /* (drawn as it was, this frame too) */
            foot->toe = b->toe_shown[best];
        } else {
            foot->pitch0 = FLAT_PITCH - 0.75f * foot->roll;
        }
        foot->roll      = 0.0f;
        foot->slap      = foot->slap_v = 0.0f;
        foot->step_v0   = vec3_zero();
        /* (how much they bother with what lies about: the sporty and the careful step round it) */
        float careless = clampf(0.08f + 0.6f * b->unsteadiness + 0.3f * (Body_clumsiness(b) - 0.5f)
                                + 0.35f * fmaxf(-b->shown.energy, 0.0f) + 0.3f * smoothstepf(2.0f, 5.0f, speed)
                                - 0.3f * (b->fitness - 0.5f), 0.0f, 0.85f);
        foot->step_to   = clearSpot(f, foot, best_spot, b->feet[1 - best].ankle, overReach(b, s), random01(b) < careless, s);
        maybeStumble(b, f, foot, speed, s); /* (drunk: may land off) */
        foot->clear_shift = vec3(foot->step_to.x - best_spot.x, 0.0f, foot->step_to.z - best_spot.z);
        foot->spot_was  = vec3_add(best_spot, foot->clear_shift);
        Vec3 rel = flat(vec3_sub(best_spot, f->position));
        foot->aim_local = vec3(vec3_dot(rel, rightOf(f->facing)), 0.0f, vec3_dot(rel, f->facing));
    }

    /* Planted feet behind the body roll onto their toes, smoothly; a foot
       that just landed on its heel puts the toes down */
    for (int i = 0; i < 2; i++) {
        Foot *foot = &b->feet[i];
        float into_was = foot->into;
        foot->into = 0.0f;
        if (foot->stepping) continue;
        float back = -vec3_dot(flat(vec3_sub(foot->ankle, f->position)), f->facing) / s;
        if (f->clutter && vec3_length(flat(vec3_sub(foot->ankle, f->position))) < LEG_NEAR * s) {
            /* now, or where the body is going (and it doesn't flicker off at once) */
            foot->into = fmaxf(legInto(f, foot->ankle, foot->dir, i, vec3_zero(), hip_y, s),
                               legInto(f, foot->ankle, foot->dir, i, vec3_scale(flat(b->velocity), LEG_AHEAD), hip_y, s));
            foot->into = fmaxf(foot->into, into_was - LEG_FORGET * dt);
        }
        /* (the leg over something behind: up on the toes, the knee goes up with the heel) */
        foot->roll = ease(foot->roll, fmaxf(smoothstepf(0.03f, 0.2f, back), foot->into > 0.0f ? 1.0f : 0.0f), 10.0f, dt);
        springCritical(&foot->slap, &foot->slap_v, 0.0f, 30.0f, dt);
    }
}

/* Half way over something: from when a foot is well on its way over it until
   the body is over there too (above where that foot came down). */
static void crossOver(Body *b, const BodyFrame *f, float s) {
    for (int i = 0; i < 2 && !b->crossing; i++) { /* (where the first foot goes over: the body goes that far, no further) */
        const Foot *ft = &b->feet[i];
        if (!ft->stepping || ft->over < CROSS_OVER * s || ft->step < CROSS_FROM) continue;
        Vec3 way = flat(vec3_sub(ft->step_to, ft->step_from));
        if (vec3_length(way) < 0.08f * s) continue; /* (lifted on the spot: not going over anything) */
        b->crossing = true;
        b->cross_foot = i;
        b->cross_to = ft->step_to;
        b->cross_dir = vec3_normalize(way);
        b->cross_left = vec3_dot(flat(vec3_sub(b->cross_to, f->position)), b->cross_dir);
        return;
    }
    if (!b->crossing) return;
    /* (a foot set a little further on, past it: the body goes after that one, no further) */
    float to = 0.0f;
    for (int i = 0; i < 2; i++) {
        const Foot *ft = &b->feet[i];
        if (!ft->stepping) to = fmaxf(to, fminf(vec3_dot(flat(vec3_sub(ft->ankle, b->cross_to)), b->cross_dir), CROSS_PAST * s));
    }
    float left = vec3_dot(flat(vec3_sub(b->cross_to, f->position)), b->cross_dir) + to;
    b->cross_left = left;
    float back = vec3_dot(flat(vec3_sub(b->feet[b->cross_foot].ankle, b->cross_to)), b->cross_dir);
    bool returned = back < -0.25f * s; /* (that foot came back: not going over after all) */
    bool away = left > CROSS_AWAY * s;  /* (the body went off the other way: not going over either - one foot was left stepping on its own) */
    if (left < CROSS_DONE * s || returned || away || b->pose != POSE_UP || b->airborne) {
        b->crossing = b->cross_force = false;
        b->cross_held = 0.0f;
    }
}

/* Where a foot lands: not on something lying there. A longer (or shorter)
   step past it, and the foot lifted over whatever is in the way. Too high to
   stand on and nowhere free: the foot stays where it is. */
static Vec3 clearSpot(const BodyFrame *f, Foot *foot, Vec3 spot, Vec3 other, float reach, bool sloppy, float s) {
    foot->over = 0.0f;
    if (!f->clutter) return spot;
    const float foot_r = 0.12f * s;
    float h = clutterAlong(f, foot->step_from, spot, 0.1f * s, 0.12f * s);
    if (spotFree(f, spot, s)) {
        foot->over = h > 0.0f ? h + OVER_CLEAR * s : 0.0f;
        return spot;
    }
    /* Not bothering (drunk, worn out, clumsy, in a hurry): onto the bottle or branch it goes */
    if (sloppy && f->clutter(f->ground_ctx, spot.x, spot.z, foot_r, f->position.y) < STEP_ON_SMALL * s) return spot;
    Vec3 right = rightOf(f->facing);
    static const float TRY[][2] = { { 0.22f, 0.0f }, { -0.16f, 0.0f }, { 0.34f, 0.0f }, { -0.3f, 0.0f },
                                    { 0.45f, 0.0f }, { 0.0f, 0.12f }, { 0.0f, -0.12f }, { -0.45f, 0.0f } };
    for (int k = 0; k < (int)(sizeof(TRY) / sizeof(TRY[0])); k++) {
        Vec3 p = vec3_add(spot, vec3_add(vec3_scale(f->facing, TRY[k][0] * s), vec3_scale(right, TRY[k][1] * s)));
        if (TRY[k][0] > 0.0f && vec3_length(flat(vec3_sub(p, other))) > reach) continue; /* (no lunge) */
        if (spotFree(f, p, s)) {
            p.y = f->ground(f->ground_ctx, p.x, p.z, spot.y + 0.4f * s) + (spot.y - f->ground(f->ground_ctx, spot.x, spot.z, spot.y + 0.4f * s));
            float hp = clutterAlong(f, foot->step_from, p, 0.1f * s, 0.12f * s);
            foot->over = hp > 0.0f ? hp + OVER_CLEAR * s : 0.0f; /* (the spot it goes to, not the one it didn't) */
            return p;
        }
    }
    float under = f->clutter(f->ground_ctx, spot.x, spot.z, foot_r, f->position.y);
    if (under > STAND_ON_MOST * s) return foot->step_from; /* nowhere free, and too high to stand on: stays */
    foot->over = h + OVER_CLEAR * s;
    return spot; /* low enough: on it then */
}

/* The body ran on while the foot waited in front of something: rather than
   landing far behind, the foot goes past it (if a leg reaches); else it stays put. */
static Vec3 leapPast(const BodyFrame *f, Foot *foot, Vec3 aim, Vec3 other, float reach, float s) {
    static const float PAST[] = { 0.22f, 0.34f, 0.45f, 0.6f };
    for (int k = 0; k < (int)(sizeof(PAST) / sizeof(PAST[0])); k++) {
        Vec3 p = vec3_add(aim, vec3_scale(f->facing, PAST[k] * s));
        if (vec3_length(flat(vec3_sub(p, other))) > reach) break;
        if (!spotFree(f, p, s)) continue;
        p.y = foot->step_to.y;
        float h = clutterAlong(f, foot->step_from, p, 0.1f * s, 0.12f * s);
        if (h > 0.0f) foot->over = fmaxf(foot->over, h + OVER_CLEAR * s);
        foot->clear_shift = vec3_add(foot->clear_shift, flat(vec3_sub(p, aim)));
        return p;
    }
    return foot->step_to;
}

Vec3 Body_takeStumblePush(Body *b) {
    Vec3 push = b->stumble_push;
    b->stumble_push = vec3_zero();
    return push;
}