#include "world/tools.h"
#include "world/object.h"

#define FELL_WORK        12.0f
#define BRANCH_WORK      1.5f   /* one branch */
#define CUT_WORK         8.0f
#define SPLIT_WORK       3.0f
#define FELL_REACH       0.75f  /* max gap between body and trunk */
#define DELIMB_REACH     0.8f
#define CUT_REACH        0.75f
#define MIN_PIECE        0.3f   /* logs aren't cut shorter than this */
#define ROUND_LENGTH     0.75f  /* logs this short are split instead of cut */
#define CUT_TOLERANCE    0.15f  /* strokes this close count for the same cut */
#define FRONT            0.3f   /* cos of the half angle a job must be in front */
#define NEARBY_REACH     0.6f   /* this much further (scaled), ... */
#define NEARBY_FRONT    -0.2f   /* ... and this far round to the side: still "at the work" (see Tools_jobNearby) */
#define BODY_HALF_DEPTH  0.15f
#define EYE_HEIGHT       1.6f   /* scaled */
#define LOOK_RANGE       2.0f   /* how far along the line of sight a stroke may aim */

/* Axe geometry (see Shapes_axe): the head sits this far below the top of the
   handle, the edge this far in front of the handle. */
#define AXE_HEAD_DROP    0.06f
#define AXE_EDGE         0.135f
#define AXE_REST_GRIP    0.24f  /* carried: the hand this far below the top, near the head */
#define AXE_END_GRIP     0.05f  /* working: the hand this far from the bottom */
#define AXE_HAND_GAP     0.12f  /* the second hand above the first */
#define READY_HOLD       1.5f   /* seconds the hands stay at the end of the handle after a stroke */
#define READY_RATE       3.5f   /* how fast the hands slide along the handle */
#define BITE_TIME        0.12f  /* the edge stays in the wood this long */
#define SAW_REACH        0.48f  /* chainsaw: from the grip to the tip of the bar */

typedef struct {
    float stroke;   /* seconds per stroke */
    float strike;   /* the tool hits at this time */
    float fell, delimb, cut, split;
    float height;   /* where a tree gets cut unless the worker looks elsewhere (scaled meters) */
    float low, high; /* range of cut heights (scaled) */
} ToolSpec;

static const ToolSpec SPECS[] = {
    [TOOL_NONE]     = { 0.0f,  0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f,  0.0f, 0.0f },
    [TOOL_HANDS]    = { 0.8f,  0.45f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f,  0.0f, 0.0f },
    [TOOL_AXE]      = { 1.15f, 0.6f,  1.0f, 1.0f, 1.0f, 1.0f, 0.95f, 0.45f, 1.35f },
    [TOOL_SHEARS]   = { 0.5f,  0.28f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,  0.0f, 0.0f },
    [TOOL_CHAINSAW] = { 0.8f,  0.3f,  3.0f, 1.0f, 2.0f, 0.0f, 0.45f, 0.3f, 0.9f },
};

static const Vec3 UP = { 0.0f, 1.0f, 0.0f };

#define AIM_UP_FROM 0.3f /* m over the eyes on the trunk: looking higher than that raises the felling cut */

static float workFor(ToolKind tool, Job job) {
    switch (job) {
    case JOB_FELL:   return SPECS[tool].fell;
    case JOB_DELIMB: return SPECS[tool].delimb;
    case JOB_CUT:    return SPECS[tool].cut;
    case JOB_SPLIT:  return SPECS[tool].split;
    case JOB_STUMP:  return SPECS[tool].fell > 0.0f ? SPECS[tool].cut : 0.0f; /* (what fells cuts the stump down) */
    case JOB_NONE:   break;
    }
    return 0.0f;
}

static float scaleOf(const Object *who) { return who->look.height / 1.75f; }

static Vec3 flat(Vec3 v) { return vec3(v.x, 0.0f, v.z); }

/* v without its part along axis, normalized (fallback if parallel). */
static Vec3 across(Vec3 v, Vec3 axis, Vec3 fallback) {
    Vec3 p = vec3_sub(v, vec3_scale(axis, vec3_dot(v, axis)));
    if (vec3_length(p) < 1e-4f) p = vec3_sub(fallback, vec3_scale(axis, vec3_dot(fallback, axis)));
    return vec3_normalize(p);
}

/* ---------- What is in hand ---------- */

ToolKind Tools_held(const Object *who, int *hand) {
    const Hands *h = &who->hands;
    if (hand) *hand = -1;
    if (h->grabbed) return TOOL_NONE;
    if (h->item[0] && h->item[0] == h->item[1])
        return h->item[0]->type == OBJ_CHAINSAW && h->state[0] == HAND_HOLDING ? TOOL_CHAINSAW : TOOL_NONE;
    for (int k = 0; k < 2; k++) {
        int i = k == 0 ? who->main_hand : 1 - who->main_hand;
        const Object *item = h->item[i];
        if (h->state[i] != HAND_HOLDING || !item) continue;
        if (item->type == OBJ_AXE || item->type == OBJ_SHEARS || item->type == OBJ_CHAINSAW) {
            if (hand) *hand = i;
            return item->type == OBJ_AXE ? TOOL_AXE : item->type == OBJ_SHEARS ? TOOL_SHEARS : TOOL_CHAINSAW;
        }
    }
    bool free = h->state[0] == HAND_FREE && h->state[1] == HAND_FREE;
    return free ? TOOL_HANDS : TOOL_NONE;
}

bool Tools_isWorking(const Object *who) {
    return who->work_state.time > 0.0f;
}

void Tools_logEnds(const Object *log, Vec3 *a, Vec3 *b) {
    const Collider *c = &log->colliders[0];
    Vec3 center = Object_colliderCenter(log, 0);
    Vec3 half = vec3_scale(c->axes[1], c->half_extents.y);
    *a = vec3_sub(center, half);
    *b = vec3_add(center, half);
}

static float logLength(const Object *o) { return 2.0f * o->colliders[0].half_extents.y; }

bool Tools_splittable(const Object *o) {
    if (o->is_held || o->collider_count == 0) return false;
    if (o->type == OBJ_FIREWOOD) return o->size.z < 4.0f;
    return o->type == OBJ_LOG && logLength(o) <= ROUND_LENGTH;
}

bool Tools_cuttable(const Object *o) {
    return o->type == OBJ_LOG && !o->is_held && o->collider_count > 0 && logLength(o) > ROUND_LENGTH;
}

float Tools_cutPoint(const Object *log, Vec3 near) {
    Vec3 a, b;
    Tools_logEnds(log, &a, &b);
    float length = logLength(log);
    float piece = 0.6f; /* splittable, and not too heavy */
    if (length - piece < MIN_PIECE) return length * 0.5f;
    bool from_a = vec3_length(vec3_sub(near, a)) <= vec3_length(vec3_sub(near, b));
    return from_a ? piece : length - piece;
}

/* ---------- Finding a job ---------- */

/* Point of collider i closest to p. */
static Vec3 closestOn(const Object *o, int i, Vec3 p) {
    const Collider *c = &o->colliders[i];
    Vec3 center = Object_colliderCenter(o, i);
    Vec3 rel = vec3_sub(p, center);
    const float half[3] = { c->half_extents.x, c->half_extents.y, c->half_extents.z };
    Vec3 q = center;
    for (int k = 0; k < 3; k++)
        q = vec3_add(q, vec3_scale(c->axes[k], clampf(vec3_dot(rel, c->axes[k]), -half[k], half[k])));
    return q;
}

static float extentUp(const Object *o) {
    const Collider *c = &o->colliders[0];
    return fabsf(c->axes[0].y) * c->half_extents.x + fabsf(c->axes[1].y) * c->half_extents.y +
           fabsf(c->axes[2].y) * c->half_extents.z;
}

typedef struct {
    Job     job;
    Object *target;
    Vec3    point;
    float   cut_at;
    float   gap;
} Plan;

static float gapTo(const Object *who, Vec3 point) {
    return vec3_length(flat(vec3_sub(point, who->position))) - BODY_HALF_DEPTH;
}

/* How strict planning is: the real thing, or "is there something to work on nearby at all". */
static float g_extra_reach = 0.0f;
static float g_front = FRONT;

static void consider(Plan *best, const Object *who, Job job, Object *target, Vec3 point, float cut_at, float reach) {
    float gap = gapTo(who, point);
    if (gap > reach + g_extra_reach || gap >= best->gap) return;
    Vec3 to = flat(vec3_sub(point, who->position));
    if (vec3_length(to) > 1e-3f && vec3_dot(vec3_normalize(to), Object_facing(who)) <= g_front) return;
    *best = (Plan){ .job = job, .target = target, .point = point, .cut_at = cut_at, .gap = gap };
}

static bool hasAim(const Object *who) {
    Vec3 a = who->work_state.aim;
    return vec3_length(a) > 0.5f;
}

static Vec3 eyeOf(const Object *who) {
    return vec3_add(who->position, vec3(0.0f, EYE_HEIGHT * scaleOf(who), 0.0f));
}

/* Height above the feet where the line of sight passes a point at this horizontal distance. */
static float lookHeight(const Object *who, float distance, float fallback) {
    if (!hasAim(who)) return fallback;
    Vec3 aim = who->work_state.aim;
    float horizontal = sqrtf(aim.x * aim.x + aim.z * aim.z);
    if (horizontal < 0.15f) return fallback;
    return EYE_HEIGHT * scaleOf(who) + aim.y * distance / horizontal;
}

/* Where the line of sight meets the height y, or a point in front. */
static Vec3 lookAtHeight(const Object *who, float y) {
    const float s = scaleOf(who);
    Vec3 front = vec3_add(who->position, vec3_scale(Object_facing(who), 0.55f * s));
    if (!hasAim(who)) return front;
    Vec3 aim = who->work_state.aim, eye = eyeOf(who);
    if (aim.y > -0.05f) return front;
    float t = (y - eye.y) / aim.y;
    if (t < 0.0f || t > LOOK_RANGE * s / fmaxf(sqrtf(aim.x * aim.x + aim.z * aim.z), 0.05f)) return front;
    return vec3_add(eye, vec3_scale(aim, t));
}

static Plan planFor(const Object *who, ToolKind tool, Object *const *objects, int count) {
    const float s = scaleOf(who);
    const Vec3 chest = vec3_add(who->position, vec3(0.0f, 1.0f * s, 0.0f));
    const ToolSpec *spec = &SPECS[tool];
    Plan best = { .job = JOB_NONE, .gap = 1e9f };

    for (int i = 0; i < count; i++) {
        Object *o = objects[i];
        if (o == who || o->is_held || o->collider_count == 0) continue;

        if (o->type == OBJ_TREE && !o->toppled && (!o->keep || who->type == OBJ_PLAYER) && workFor(tool, JOB_FELL) > 0.0f) { /* (you may fell any tree - some cost dear: forestry.c) */
            /* The cut goes in where the worker looks, within the tool's range */
            Vec3 c = Object_colliderCenter(o, 0);
            float distance = vec3_length(flat(vec3_sub(c, who->position)));
            /* About hip high. Looking lower: right where you look. Looking straight ahead isn't aiming
               up the trunk; looking up past AIM_UP_FROM over the eyes, the cut goes up that much */
            float look = lookHeight(who, distance, spec->height * s), hip = spec->height * s, from = (EYE_HEIGHT + AIM_UP_FROM) * s;
            float h = clampf(look <= hip ? look : look >= from ? hip + (look - from) : hip, spec->low * s, spec->high * s);
            float top = o->position.y + 2.0f * o->colliders[0].half_extents.y - 0.3f;
            float y = who->position.y + h;
            /* A cut already started: keep hitting the same notch, whoever swings, as long as it's in reach */
            float notch = o->position.y + o->cut_at - who->position.y;
            if (o->work > 0.0f && notch >= spec->low * s - 0.05f && notch <= spec->high * s + 0.05f) y = o->position.y + o->cut_at;
            Vec3 at = vec3(who->position.x, clampf(y, o->position.y + 0.1f, top), who->position.z);
            consider(&best, who, JOB_FELL, o, closestOn(o, 0, at), 0.0f, FELL_REACH);
        } else if (o->type == OBJ_TREE && o->toppled && o->topple >= 1.0f && workFor(tool, JOB_DELIMB) > 0.0f) {
            for (int c = 1; c < o->collider_count; c++) /* the crown */
                consider(&best, who, JOB_DELIMB, o, closestOn(o, c, chest), 0.0f, DELIMB_REACH);
        } else if (o->type == OBJ_STUMP && workFor(tool, JOB_STUMP) > 0.0f) {
            /* Chopped down from above, into the middle */
            Vec3 c = Object_colliderCenter(o, 0);
            consider(&best, who, JOB_STUMP, o, vec3(c.x, c.y + o->colliders[0].half_extents.y, c.z), 0.0f, CUT_REACH);
        } else if (o->type == OBJ_LOG || o->type == OBJ_FIREWOOD) {
            Vec3 a, b;
            Tools_logEnds(o, &a, &b);
            Vec3 axis = vec3_sub(b, a);
            float length = vec3_length(axis);
            axis = vec3_scale(axis, 1.0f / fmaxf(length, 1e-4f));
            float top = Object_colliderCenter(o, 0).y + extentUp(o);
            Vec3 aim = lookAtHeight(who, top);
            bool short_piece = o->type == OBJ_FIREWOOD || length <= ROUND_LENGTH;
            if (short_piece) {
                /* Split along its length, from above */
                if (o->type == OBJ_FIREWOOD && o->size.z >= 4.0f) continue; /* quarters: done */
                if (workFor(tool, JOB_SPLIT) <= 0.0f) continue;
                Vec3 middle = vec3_add(a, vec3_scale(axis, length * 0.5f));
                consider(&best, who, JOB_SPLIT, o, vec3(middle.x, top, middle.z), 0.0f, CUT_REACH);
            } else {
                if (workFor(tool, JOB_CUT) <= 0.0f) continue;
                float t = clampf(vec3_dot(vec3_sub(aim, a), axis), MIN_PIECE, length - MIN_PIECE);
                if (o->work > 0.0f && fabsf(o->cut_at - t) < 0.5f) t = o->cut_at; /* same cut as before */
                Vec3 on_axis = vec3_add(a, vec3_scale(axis, t));
                consider(&best, who, JOB_CUT, o, vec3(on_axis.x, top, on_axis.z), t, CUT_REACH);
            }
        }
    }
    return best;
}

bool Tools_hasJob(const Object *who, Object *const *objects, int count) {
    ToolKind tool = Tools_held(who, NULL);
    return tool != TOOL_NONE && planFor(who, tool, objects, count).job != JOB_NONE;
}

bool Tools_jobNearby(const Object *who, Object *const *objects, int count) {
    ToolKind tool = Tools_held(who, NULL);
    if (tool == TOOL_NONE || tool == TOOL_HANDS) return false;
    g_extra_reach = NEARBY_REACH * scaleOf(who);
    g_front = NEARBY_FRONT;
    bool near = planFor(who, tool, objects, count).job != JOB_NONE;
    g_extra_reach = 0.0f;
    g_front = FRONT;
    return near;
}

Object *Tools_target(const Object *who, Object *const *objects, int count) {
    ToolKind tool = Tools_held(who, NULL);
    return tool != TOOL_NONE ? planFor(who, tool, objects, count).target : NULL;
}

bool Tools_use(Object *who, Object *const *objects, int count) {
    Work *w = &who->work_state;
    if (w->time > 0.0f || who->hands.drink_hand >= 0) return false;
    int hand;
    ToolKind tool = Tools_held(who, &hand);
    if (tool == TOOL_NONE) return false;
    if (tool == TOOL_CHAINSAW && hand >= 0 && who->hands.state[1 - hand] != HAND_FREE) return false; /* needs both hands */

    Plan plan = planFor(who, tool, objects, count);
    if (tool == TOOL_HANDS && plan.job == JOB_NONE) return false; /* nothing to pull at */

    const float s = scaleOf(who);
    if (plan.job != JOB_NONE) {
        w->last_job = plan.job;
        w->last_target = plan.target;
        w->since = 0.0f;
    }
    w->time   = 1e-4f;
    w->tool   = tool;
    w->hand   = hand;
    w->job    = plan.job;
    w->target = plan.target;
    w->cut_at = plan.cut_at;
    w->point  = plan.job != JOB_NONE ? plan.point
              : Figure_toWorld(who->position, Object_facing(who), vec3(0.0f, 1.0f * s, 0.75f * s)); /* air */
    return true;
}

/* ---------- Doing the work ---------- */

static bool stillThere(const Work *w) {
    const Object *o = w->target;
    if (!o) return false;
    switch (w->job) {
    case JOB_FELL:   return o->type == OBJ_TREE && !o->toppled;
    case JOB_DELIMB: return o->type == OBJ_TREE && o->toppled && o->topple >= 1.0f;
    case JOB_CUT:    return o->type == OBJ_LOG && !o->is_held;
    case JOB_SPLIT:  return (o->type == OBJ_LOG || (o->type == OBJ_FIREWOOD && o->size.z < 4.0f)) && !o->is_held;
    case JOB_STUMP:  return o->type == OBJ_STUMP && !o->broken;
    case JOB_NONE:   break;
    }
    return false;
}

static void finish(Work *w, float at) {
    w->jobs_done++;
    if (w->done_count < WORK_DONE_MAX)
        w->done[w->done_count++] = (JobDone){ .job = w->job, .target = w->target, .at = at, .point = w->point };
}

float Tools_fellDone(const Object *tree) { return clampf(tree->work / FELL_WORK, 0.0f, 1.0f); }

static void strike(Work *w) {
    float amount = workFor(w->tool, w->job) * w->power; /* how well the worker is doing */
    if (amount <= 0.0f || !stillThere(w)) return;
    Object *o = w->target;
    w->impact = 0.18f;
    w->struck++;
    switch (w->job) {
    case JOB_FELL:
        /* the tree comes apart where it's hit: a new notch elsewhere starts over
           (never felled at a spot hit only once) */
        if (o->work > 0.0f && fabsf(w->point.y - o->position.y - o->cut_at) > CUT_TOLERANCE) o->work = 0.0f;
        o->cut_at = w->point.y - o->position.y;
        if ((o->work += amount) >= FELL_WORK) {
            o->work = 0.0f;
            Object_markCut(o, w->point, 0.0f);
            finish(w, o->cut_at);
        } else {
            Object_markCut(o, w->point, o->work / FELL_WORK);
        }
        break;
    case JOB_DELIMB:
        /* One branch at a time; strokes of everyone working on it add up */
        o->work += amount;
        while (o->work >= BRANCH_WORK) {
            o->work -= BRANCH_WORK;
            finish(w, 0.0f);
        }
        break;
    case JOB_CUT:
        /* A stroke somewhere else starts a new cut */
        if (fabsf(o->cut_at - w->cut_at) > CUT_TOLERANCE) o->work = 0.0f;
        o->cut_at = w->cut_at;
        if ((o->work += amount) >= CUT_WORK) {
            o->work = 0.0f;
            Object_markCut(o, w->point, 0.0f);
            finish(w, w->cut_at);
        } else {
            Object_markCut(o, w->point, o->work / CUT_WORK);
        }
        break;
    case JOB_SPLIT:
        if ((o->work += amount) >= SPLIT_WORK) {
            o->work = 0.0f;
            finish(w, 0.0f);
        }
        break;
    case JOB_STUMP:
        if ((o->work += amount) >= STUMP_WORK) finish(w, 0.0f);
        Object_markCut(o, w->point, fminf(o->work / STUMP_WORK, 1.0f));
        break;
    case JOB_NONE:
        break;
    }
}

bool Tools_takeDone(Object *who, JobDone *done) {
    Work *w = &who->work_state;
    if (w->done_count == 0) return false;
    *done = w->done[0];
    for (int i = 1; i < w->done_count; i++) w->done[i - 1] = w->done[i];
    w->done_count--;
    return true;
}

bool Tools_canDo(ToolKind tool, Job job) {
    return workFor(tool, job) > 0.0f;
}

/* ---------- Moving the arms ---------- */

/* Smooth path carry -> wind-up -> strike -> follow-through -> carry. */
static Vec3 strokePath(float t, const ToolSpec *spec, Vec3 carry, Vec3 windup, Vec3 hit, Vec3 follow) {
    float wind = spec->strike * 0.7f, after = spec->strike + 0.12f;
    if (t < wind) return vec3_lerp(carry, windup, smoothstepf(0.0f, wind, t));
    if (t < spec->strike) return vec3_lerp(windup, hit, smoothstepf(wind, spec->strike, t));
    if (t < after) return vec3_lerp(hit, follow, smoothstepf(spec->strike, after, t));
    return vec3_lerp(follow, carry, smoothstepf(after, spec->stroke, t));
}

static void reach(Body *b, int hand, Vec3 world) {
    b->hand_active[hand] = true;
    b->hand_target[hand] = world;
}

/* ----- The axe ----- */

/* How the axe is held, in figure space: where the (lower) hand is, where the
   handle points (towards the head) and the edge faces, and where along the
   handle the hand grips it. */
typedef struct {
    Vec3  hand;
    Vec3  handle;
    Vec3  blade;
    float grip;
    float contact;  /* 0..1: the edge sits at edge */
    Vec3  edge;
} AxePose;

static AxePose blendPose(AxePose a, AxePose b, float k) {
    AxePose p = a;
    p.hand   = vec3_lerp(a.hand, b.hand, k);
    p.handle = vec3_normalize(vec3_lerp(a.handle, b.handle, k));
    p.blade  = across(vec3_lerp(a.blade, b.blade, k), p.handle, b.blade);
    p.grip   = lerpf(a.grip, b.grip, k);
    p.contact = 0.0f;
    return p;
}

/* A swing turns the axe on an arc: around the vertical for a horizontal
   swing at a trunk, around the shoulders' axis for a stroke from above. */
typedef struct {
    bool  overhead;
    Vec3  forward;     /* overhead: horizontal direction of the stroke */
    float sign;        /* side of the hand at the end of the handle */
} Arc;

/* Handle and edge direction at an angle on the arc. The edge always faces
   the way the axe moves. */
static void arcAt(const Arc *arc, float angle, Vec3 *handle, Vec3 *blade) {
    if (arc->overhead) {
        /* angle 0: handle forward, > 0 raised; moving down */
        *handle = vec3_add(vec3_scale(arc->forward, cosf(angle)), vec3_scale(UP, sinf(angle)));
        *blade  = vec3_sub(vec3_scale(arc->forward, sinf(angle)), vec3_scale(UP, cosf(angle)));
    } else {
        /* angle 0: handle forward, > 0 towards the right; a right-hander swings from right to left */
        *handle = vec3(sinf(angle), 0.0f, cosf(angle));
        *blade  = vec3_scale(vec3(cosf(angle), 0.0f, -sinf(angle)), -arc->sign);
    }
}

/* Hand position that puts the edge at e for this arc angle. */
static Vec3 handFor(const Arc *arc, float angle, Vec3 e, float reach_a) {
    Vec3 u, v;
    arcAt(arc, angle, &u, &v);
    return vec3_sub(vec3_sub(e, vec3_scale(u, reach_a)), vec3_scale(v, AXE_EDGE));
}

/* Arc angle at the moment of the hit: the hands end up comfortably in front. */
static float hitAngle(const Arc *arc, Vec3 e, float reach_a, float s) {
    float best = 0.0f, best_cost = 1e9f;
    float from = arc->overhead ? -1.45f : -1.4f, to = arc->overhead ? 0.5f : 1.4f;
    for (float a = from; a <= to; a += 0.04f) {
        Vec3 h = handFor(arc, a, e, reach_a);
        float cost;
        if (arc->overhead) {
            float ahead = vec3_dot(flat(h), arc->forward);
            cost = 0.5f * (h.y - 0.75f * s) * (h.y - 0.75f * s) + 1.5f * (ahead - 0.32f * s) * (ahead - 0.32f * s)
                 + 20.0f * fmaxf(0.0f, 0.18f * s - ahead) * fmaxf(0.0f, 0.18f * s - ahead)
                 + 20.0f * fmaxf(0.0f, 0.35f * s - h.y) * fmaxf(0.0f, 0.35f * s - h.y);
        } else {
            cost = (h.x - arc->sign * 0.06f * s) * (h.x - arc->sign * 0.06f * s)
                 + 1.5f * (h.z - 0.32f * s) * (h.z - 0.32f * s)
                 + 20.0f * fmaxf(0.0f, 0.2f * s - h.z) * fmaxf(0.0f, 0.2f * s - h.z);
        }
        if (cost < best_cost) {
            best_cost = cost;
            best = a;
        }
    }
    return best;
}

static AxePose arcPose(const Arc *arc, float angle, Vec3 hand, float grip) {
    AxePose p = { .hand = hand, .grip = grip };
    arcAt(arc, angle, &p.handle, &p.blade);
    return p;
}

/* Wind up, swing, bite into the wood and come back to where the stroke began. */
static AxePose axeStroke(const Object *who, const ToolSpec *spec, AxePose start, float sign,
                         float length, float s) {
    const Work *w = &who->work_state;
    const float t = w->time, strike_at = spec->strike;
    const float end_grip = -length * 0.5f + AXE_END_GRIP;
    const float reach_a = length - AXE_HEAD_DROP - AXE_END_GRIP; /* hand to head, along the handle */
    Vec3 e = Figure_toLocal(who->position, Object_facing(who), w->point);

    Arc arc = { .sign = sign };
    arc.overhead = w->job == JOB_CUT || w->job == JOB_SPLIT || w->job == JOB_STUMP;
    arc.forward = vec3_length(flat(e)) > 1e-3f ? vec3_normalize(flat(e)) : vec3(0.0f, 0.0f, 1.0f);
    if (!arc.overhead) e.y = clampf(e.y, 0.45f * s, 1.4f * s); /* hands at the height of the cut */

    float hit = hitAngle(&arc, e, reach_a, s);
    float wind = arc.overhead ? 2.4f : hit + sign * 2.0f;
    float follow = arc.overhead ? hit - 0.6f : hit - sign * 0.9f;
    Vec3 hit_hand = handFor(&arc, hit, e, reach_a);
    Vec3 wind_hand = arc.overhead ? vec3(sign * 0.05f * s, 1.72f * s, 0.0f)
                                  : vec3(sign * 0.3f * s, e.y + 0.1f * s, 0.06f * s);
    AxePose windup = arcPose(&arc, wind, wind_hand, end_grip);
    AxePose struck = arcPose(&arc, hit, hit_hand, end_grip);
    bool air = w->job == JOB_NONE;

    const float wind_end = strike_at * 0.62f, bite_end = strike_at + BITE_TIME;
    if (t < wind_end) return blendPose(start, windup, smoothstepf(0.0f, wind_end, t));
    if (t < strike_at) {
        /* Speeding up into the hit */
        float k = (t - wind_end) / (strike_at - wind_end);
        k *= k;
        AxePose p = arcPose(&arc, lerpf(wind, hit, k), vec3_lerp(wind_hand, hit_hand, k), end_grip);
        if (!air && k > 0.8f) {
            p.contact = (k - 0.8f) / 0.2f;
            p.edge = e;
        }
        return p;
    }
    if (air) {
        /* Nothing there: the axe swings on through */
        float k = smoothstepf(strike_at, bite_end, t);
        Vec3 through = handFor(&arc, follow, e, reach_a);
        AxePose p = arcPose(&arc, lerpf(hit, follow, k), vec3_lerp(hit_hand, through, k), end_grip);
        if (t < bite_end) return p;
        AxePose done = arcPose(&arc, follow, through, end_grip);
        return blendPose(done, start, smoothstepf(bite_end, spec->stroke, t));
    }
    if (t < bite_end) {
        /* The edge is stuck in the wood; the hands jolt back a little */
        AxePose p = struck;
        float k = (t - strike_at) / BITE_TIME;
        p.hand = vec3_sub(p.hand, vec3_scale(p.handle, 0.015f * s * sinf(PI_F * k)));
        p.contact = 1.0f;
        p.edge = e;
        return p;
    }
    /* Pulled straight out of the cut, turned away from the wood, then back to the start */
    const float out_end = bite_end + 0.07f, away_end = out_end + 0.14f;
    Vec3 out_hand = vec3_sub(hit_hand, vec3_scale(struck.handle, 0.22f * s));
    if (t < out_end) {
        AxePose p = struck;
        p.hand = vec3_lerp(hit_hand, out_hand, smoothstepf(bite_end, out_end, t));
        p.contact = 1.0f - smoothstepf(bite_end, bite_end + 0.04f, t);
        p.edge = e;
        return p;
    }
    float back = arc.overhead ? hit + 1.2f : hit + sign * 1.6f;
    Vec3 near_hand = arc.overhead ? vec3(sign * 0.05f * s, 1.0f * s, 0.2f * s)
                                  : vec3(sign * 0.22f * s, e.y, 0.18f * s);
    AxePose pulled = arcPose(&arc, back, near_hand, end_grip);
    if (t < away_end) {
        float k = smoothstepf(out_end, away_end, t);
        return arcPose(&arc, lerpf(hit, back, k), vec3_lerp(out_hand, near_hand, k), end_grip);
    }
    return blendPose(pulled, start, smoothstepf(away_end, spec->stroke, t));
}

/* Carried near the head, or ready at the end of the handle; strokes start
   and end in the ready pose. */
static void holdAxe(Object *who, int hand, float dt) {
    Work *w = &who->work_state;
    Body *b = &who->body;
    const Object *axe = who->hands.item[hand];
    const float s = scaleOf(who), sign = hand == HAND_LEFT ? -1.0f : 1.0f;
    const float length = axe->size.y, half = length * 0.5f;
    const Vec3 pos = who->position, facing = Object_facing(who);

    bool ready_goal = w->time > 0.0f || w->since < READY_HOLD;
    w->ready = clampf(w->ready + (ready_goal ? 1.0f : -1.0f) * READY_RATE * dt, 0.0f, 1.0f);

    AxePose rest = { .hand = vec3(sign * 0.24f * s, 0.80f * s, 0.10f * s),
                     .handle = vec3_normalize(vec3(sign * 0.15f, 1.0f, 0.55f)),
                     .grip = half - AXE_REST_GRIP };
    rest.blade = across(vec3(0.0f, 0.0f, 1.0f), rest.handle, UP);
    /* Ready: across the body, head up by the other shoulder, clear of what's in front */
    AxePose ready = { .hand = vec3(sign * 0.15f * s, 0.92f * s, 0.2f * s),
                      .handle = vec3_normalize(vec3(-sign * 0.6f, 1.0f, -0.05f)),
                      .grip = -half + AXE_END_GRIP };
    ready.blade = across(vec3(-sign, 0.0f, 0.0f), ready.handle, UP);
    AxePose pose = blendPose(rest, ready, smoothstepf(0.0f, 1.0f, w->ready));

    if (w->time > 0.0f) {
        const ToolSpec *spec = &SPECS[TOOL_AXE];
        pose = axeStroke(who, spec, pose, sign, length, s);
        float effort = smoothstepf(0.0f, spec->strike, w->time) *
                       (1.0f - smoothstepf(spec->strike + 0.1f, spec->stroke, w->time));
        b->crouch_goal = fmaxf(b->crouch_goal, clampf((0.95f * s - pose.hand.y) / (0.75f * s), 0.0f, 1.0f));
        b->lean_goal = fmaxf(b->lean_goal, 0.25f * effort);
        b->chin_goal = -0.1f * effort;
    }

    reach(b, hand, Figure_toWorld(pos, facing, pose.hand));
    int other = 1 - hand;
    if (who->hands.state[other] == HAND_FREE && (w->ready > 0.25f || w->time > 0.0f))
        reach(b, other, Figure_toWorld(pos, facing, vec3_add(pose.hand, vec3_scale(pose.handle, AXE_HAND_GAP))));

    w->handle  = Figure_toWorld(vec3_zero(), facing, pose.handle);
    w->blade   = Figure_toWorld(vec3_zero(), facing, pose.blade);
    w->grip    = pose.grip;
    w->contact = pose.contact;
    w->edge    = Figure_toWorld(pos, facing, pose.edge);
}

/* ----- Other tools ----- */

static void useShears(Object *who, const ToolSpec *spec, Vec3 hit) {
    Work *w = &who->work_state;
    Body *b = &who->body;
    const float s = scaleOf(who), t = w->time;
    const Vec3 pos = who->position, facing = Object_facing(who);
    int i = w->hand;
    float sign = i == HAND_LEFT ? -1.0f : 1.0f;
    Vec3 carry = Figure_toLocal(pos, facing, b->hand_target[i]);
    Vec3 approach = vec3_add(hit, vec3(0.0f, 0.05f * s, -0.15f * s));
    reach(b, i, Figure_toWorld(pos, facing, strokePath(t, spec, carry, approach, hit, hit)));
    int o = 1 - i; /* the other hand holds the branch */
    if (who->hands.state[o] == HAND_FREE)
        reach(b, o, Figure_toWorld(pos, facing, vec3_add(hit, vec3(-sign * 0.12f * s, 0.05f * s, 0.0f))));
}

/* Both hands on the saw: the bar goes into the wood, saws a moment, comes back. */
static void useChainsaw(Object *who, const ToolSpec *spec) {
    Work *w = &who->work_state;
    Body *b = &who->body;
    const float s = scaleOf(who), t = w->time;
    const Vec3 pos = who->position, facing = Object_facing(who);
    Vec3 e = Figure_toLocal(pos, facing, w->point);
    Vec3 forward = vec3_length(flat(e)) > 1e-3f ? vec3_normalize(flat(e)) : vec3(0.0f, 0.0f, 1.0f);
    bool low = w->job == JOB_CUT || w->job == JOB_STUMP;
    Vec3 dir = low ? vec3_normalize(vec3_sub(vec3_scale(forward, 0.8f), vec3_scale(UP, 0.6f))) : forward;

    float in = smoothstepf(0.0f, spec->strike, t) * (1.0f - smoothstepf(spec->stroke * 0.8f, spec->stroke, t));
    float thickness = w->target ? 2.0f * w->target->colliders[0].half_extents.x : 0.2f;
    Vec3 tip = vec3_add(e, vec3_scale(low ? vec3_scale(UP, -1.0f) : forward, in * 0.7f * thickness));
    Vec3 grip = vec3_sub(tip, vec3_scale(dir, SAW_REACH));
    grip.z = fmaxf(grip.z, 0.15f * s);
    float buzz = sinf(t * 90.0f) * 0.006f * s;
    for (int i = 0; i < 2; i++) {
        /* The main hand on the rear handle, the other one on the front grip */
        float sign = i == HAND_LEFT ? -1.0f : 1.0f;
        Vec3 carry = who->skeleton.joints[i == HAND_LEFT ? JOINT_HAND_L : JOINT_HAND_R];
        Vec3 at = vec3_add(grip, vec3(sign * 0.09f * s, buzz, 0.0f));
        reach(b, i, Figure_toWorld(pos, facing, vec3_lerp(carry, at, smoothstepf(0.0f, 1.0f, in * 1.5f))));
    }
    float hands_y = grip.y;
    b->crouch_goal = fmaxf(b->crouch_goal, clampf((0.95f * s - hands_y) / (0.75f * s), 0.0f, 1.0f) * in);
    b->lean_goal = fmaxf(b->lean_goal, 0.3f * in);
    w->saw_dir = Figure_toWorld(vec3_zero(), facing, dir);
    w->saw_in = in;
    if (in > 0.6f && w->job != JOB_NONE) w->impact = fmaxf(w->impact, 0.05f); /* the saw shakes */
}

/* Grab the branches and tear them off towards you. */
static void useHands(Object *who, const ToolSpec *spec, Vec3 hit) {
    Body *b = &who->body;
    const float s = scaleOf(who), t = who->work_state.time;
    const Vec3 pos = who->position, facing = Object_facing(who);
    for (int i = 0; i < 2; i++) {
        float sign = i == HAND_LEFT ? -1.0f : 1.0f;
        Vec3 carry = vec3(sign * 0.22f * s, 0.85f * s, 0.15f * s);
        Vec3 grab = vec3_add(hit, vec3(sign * 0.08f * s, 0.0f, 0.0f));
        Vec3 pulled = vec3(sign * 0.15f * s, hit.y + 0.15f * s, 0.22f * s);
        reach(b, i, Figure_toWorld(pos, facing, strokePath(t, spec, carry, grab, grab, pulled)));
    }
    float effort = smoothstepf(0.0f, spec->strike, t);
    b->lean_goal = 0.3f * effort - 0.45f * smoothstepf(spec->strike, spec->stroke, t); /* lean back to pull */
}

void Tools_update(Object *who, float dt) {
    Work *w = &who->work_state;
    w->power = who->work_rate > 0.0f ? who->work_rate : 1.0f;
    w->since += dt;
    w->impact = fmaxf(0.0f, w->impact - dt);
    w->contact = 0.0f;
    w->saw_in = 0.0f;
    int hand;
    ToolKind held = Tools_held(who, &hand);
    if (w->time > 0.0f && held != w->tool) w->time = 0.0f; /* dropped the tool */

    if (w->time > 0.0f) {
        const ToolSpec *spec = &SPECS[w->tool];
        float before = w->time;
        w->time += dt * Object_vigor(who) * (who->tempo > 0.0f ? who->tempo : 1.0f); /* tired: slower strokes; and everyone's own pace */
        if (before < spec->strike && w->time >= spec->strike) strike(w);
        if (w->time >= spec->stroke) w->time = 0.0f;
    }

    if (held == TOOL_AXE) {
        holdAxe(who, hand, dt);
        return;
    }
    w->ready = 0.0f;
    if (w->time <= 0.0f) return;

    const ToolSpec *spec = &SPECS[w->tool];
    Body *b = &who->body;
    const float s = scaleOf(who);
    /* Where the stroke lands, within reach of the arms */
    Vec3 hit = Figure_toLocal(who->position, Object_facing(who), w->point);
    hit.x = clampf(hit.x, -0.35f * s, 0.35f * s);
    hit.z = clampf(hit.z, 0.3f * s, 0.62f * s);
    hit.y = fmaxf(hit.y, 0.12f * s);
    float effort = smoothstepf(0.0f, spec->strike, w->time) * (1.0f - smoothstepf(spec->strike + 0.1f, spec->stroke, w->time));
    b->crouch_goal = fmaxf(b->crouch_goal, clampf((0.95f * s - hit.y) / (0.75f * s), 0.0f, 1.0f) * effort);
    b->lean_goal = fmaxf(b->lean_goal, 0.3f * effort);

    switch (w->tool) {
    case TOOL_SHEARS:   useShears(who, spec, hit); break;
    case TOOL_CHAINSAW: useChainsaw(who, spec);    break;
    case TOOL_HANDS:    useHands(who, spec, hit);  break;
    case TOOL_AXE:
    case TOOL_NONE:     break;
    }
}

/* ---------- Tools in the hand ---------- */

static Vec3 jointWorld(const Object *who, int joint) {
    return Figure_toWorld(who->position, Object_facing(who), who->skeleton.joints[joint]);
}

/* Puts an item with its local Y along y_axis and local Z along z_axis, so
   that the local point anchor (relative to the mesh origin) is at world. */
static void poseItem(Object *item, Vec3 y_axis, Vec3 z_axis, Vec3 anchor, Vec3 world) {
    item->rotation = euler_yxz_from_axes(y_axis, z_axis);
    Object_syncColliders(item);
    Vec3 origin = vec3_sub(world, mat4_mul_dir(mat4_rotate_yxz(item->rotation), anchor));
    item->position = vec3_sub(origin, item->pivot);
    item->velocity = vec3_zero();
}

/* Turns the axe around the hand (k = 0..1 of the way) so the edge lands on point;
   the hand slides a little along the handle if the arms fall short. */
static void edgeTo(Vec3 grip_at, Vec3 point, float half, float k, Vec3 *u, Vec3 *v, float *g) {
    Vec3 n = vec3_cross(*u, *v);
    Vec3 e = vec3_sub(point, grip_at);
    e = vec3_sub(e, vec3_scale(n, vec3_dot(e, n)));
    float d = vec3_length(e);
    if (d < AXE_EDGE + 0.05f) return;
    float a = sqrtf(d * d - AXE_EDGE * AXE_EDGE);
    Vec3 along = vec3_scale(e, 1.0f / d);
    Vec3 side = across(*v, along, n);
    Vec3 u_hit = vec3_scale(vec3_sub(vec3_scale(along, a), vec3_scale(side, AXE_EDGE)), 1.0f / d);
    Vec3 v_hit = vec3_scale(vec3_add(vec3_scale(along, AXE_EDGE), vec3_scale(side, a)), 1.0f / d);
    float g_hit = clampf(half - AXE_HEAD_DROP - a, -half + 0.02f, half - 0.15f);
    *u = vec3_normalize(vec3_lerp(*u, u_hit, k));
    *v = across(vec3_lerp(*v, v_hit, k), *u, v_hit);
    *g = lerpf(*g, g_hit, k);
}

/* Inside collider i of o? Then *out is the nearest point on its surface. */
static bool insideOf(const Object *o, int i, Vec3 p, Vec3 *out) {
    const Collider *c = &o->colliders[i];
    Vec3 rel = vec3_sub(p, Object_colliderCenter(o, i));
    const float half[3] = { c->half_extents.x, c->half_extents.y, c->half_extents.z };
    int axis = -1;
    float least = 1e9f, d[3];
    for (int k = 0; k < 3; k++) {
        d[k] = vec3_dot(rel, c->axes[k]);
        float room = half[k] - fabsf(d[k]);
        if (room <= 0.0f) return false;
        if (room < least) { least = room; axis = k; }
    }
    *out = vec3_add(p, vec3_scale(c->axes[axis], d[axis] >= 0.0f ? least : -least));
    return true;
}

/* The axe: handle through the hand. In the wood the edge sits exactly where it
   bit, and it never passes through what is being worked on. */
static void placeAxe(Object *who, int hand, Object *item) {
    const Work *w = &who->work_state;
    Vec3 grip_at = jointWorld(who, hand == HAND_LEFT ? JOINT_HAND_L : JOINT_HAND_R);
    const float half = item->size.y * 0.5f;
    Vec3 u = vec3_length(w->handle) > 0.5f ? w->handle : UP;
    Vec3 v = across(w->blade, u, Object_facing(who));
    float g = w->grip;

    if (w->contact > 0.0f) edgeTo(grip_at, w->edge, half, w->contact, &u, &v, &g);

    const Object *wood = w->target;
    if (w->time > 0.0f && wood && wood->collider_count > 0 && w->job != JOB_DELIMB) {
        Vec3 edge = vec3_add(grip_at, vec3_add(vec3_scale(u, half - AXE_HEAD_DROP - g), vec3_scale(v, AXE_EDGE)));
        Vec3 surface;
        if (insideOf(wood, 0, edge, &surface)) edgeTo(grip_at, surface, half, 1.0f, &u, &v, &g);
    }
    poseItem(item, u, v, vec3(0.0f, g, 0.0f), grip_at);
}

static void placeShears(Object *who, int hand, Object *item) {
    const Work *w = &who->work_state;
    Vec3 wrist = jointWorld(who, hand == HAND_LEFT ? JOINT_WRIST_L : JOINT_WRIST_R);
    Vec3 palm  = jointWorld(who, hand == HAND_LEFT ? JOINT_HAND_L : JOINT_HAND_R);
    const float half = item->colliders[0].half_extents.y;
    Vec3 forearm = vec3_normalize(vec3_sub(palm, wrist));
    Vec3 aim = w->time > 0.0f ? vec3_normalize(vec3_sub(w->point, palm)) : Object_facing(who);
    Vec3 handle = w->time > 0.0f ? vec3_normalize(vec3_lerp(forearm, aim, 0.6f)) : forearm;
    poseItem(item, handle, across(aim, handle, UP), vec3(0.0f, -(half - 0.1f), 0.0f), palm);
}

/* Where the mesh origin goes and how it turns, for an item posed by axes and an anchor point. */
static void poseOf(Vec3 y_axis, Vec3 z_axis, Vec3 anchor, Vec3 world, Vec3 *rotation, Vec3 *origin) {
    *rotation = euler_yxz_from_axes(y_axis, z_axis);
    *origin = vec3_sub(world, mat4_mul_dir(mat4_rotate_yxz(*rotation), anchor));
}

/* Chainsaw: carried relaxed in one hand by the top handle, the bar forward and
   a little down; while sawing both hands hold it and the bar goes into the wood. */
static void placeChainsaw(Object *who, int hand, Object *item) {
    const Work *w = &who->work_state;
    const Vec3 facing = Object_facing(who);
    Vec3 palm = jointWorld(who, hand == HAND_LEFT ? JOINT_HAND_L : JOINT_HAND_R);
    Vec3 bar = vec3_normalize(vec3_sub(facing, vec3_scale(UP, 0.35f)));
    Vec3 carried_rot, carried_at;
    poseOf(across(UP, bar, facing), vec3_scale(bar, -1.0f), vec3(0.0f, 0.15f, 0.24f), palm, &carried_rot, &carried_at);

    float k = smoothstepf(0.0f, 1.0f, clampf(w->saw_in * 1.5f, 0.0f, 1.0f));
    Vec3 rotation = carried_rot, origin = carried_at;
    if (k > 0.0f && vec3_length(w->saw_dir) > 0.5f) {
        Vec3 grip_at = vec3_scale(vec3_add(jointWorld(who, JOINT_HAND_L), jointWorld(who, JOINT_HAND_R)), 0.5f);
        Vec3 dir = w->saw_dir, working_rot, working_at;
        poseOf(across(UP, dir, facing), vec3_scale(dir, -1.0f), vec3(0.0f, 0.05f, 0.1f), grip_at, &working_rot, &working_at);
        Mat4 a = mat4_rotate_yxz(carried_rot), b = mat4_rotate_yxz(working_rot);
        Vec3 y = vec3_normalize(vec3_lerp(mat4_mul_dir(a, UP), mat4_mul_dir(b, UP), k));
        Vec3 z = across(vec3_lerp(mat4_mul_dir(a, vec3(0.0f, 0.0f, 1.0f)), mat4_mul_dir(b, vec3(0.0f, 0.0f, 1.0f)), k), y, facing);
        rotation = euler_yxz_from_axes(y, z);
        origin = vec3_lerp(carried_at, working_at, k);
    }
    item->rotation = rotation;
    Object_syncColliders(item);
    item->position = vec3_sub(origin, item->pivot);
    item->velocity = vec3_zero();
}

bool Tools_placeHeld(Object *who, int hand, Object *item) {
    const Hands *h = &who->hands;
    switch (item->type) {
    case OBJ_AXE:
        if (Tools_held(who, NULL) != TOOL_AXE) return false;
        placeAxe(who, hand, item);
        return true;
    case OBJ_SHEARS:
        placeShears(who, hand, item);
        return true;
    case OBJ_CHAINSAW:
        if (h->item[0] == item && h->item[1] == item) return false; /* an old two-handed carry */
        placeChainsaw(who, hand, item);
        return true;
    default:
        return false;
    }
}