#include "people/dog.h"
#include "world/world.h"

#include <string.h>

extern struct World *g_world; /* (ai.c: the world they live in) */

enum { DOG_WANDER, DOG_FOLLOW, DOG_COME, DOG_BEG, DOG_REST, DOG_FETCH, DOG_BRING, DOG_KEEP };

#define TROT_SPEED   1.5f
#define RUN_SPEED    3.4f
#define FOLLOW_FAR   9.0f   /* further from you than this: she catches up */
#define BEG_RANGE    7.0f   /* smells a sausage this far */
#define FIRE_RANGE  25.0f
#define TURN_RATE    6.0f
#define FETCH_SEES  20.0f   /* m: something thrown this near, she's after it */
#define FOOD_SMELLS 10.0f   /* m: a sausage lying on the ground this near: hers */
#define FETCH_KG     2.0f   /* things up to this she carries in her mouth */
#define CATCH_HIGH   0.75f  /* m over the ground: in the air up to this she may snap it */
#define CATCH_CHANCE 0.55f
#define CHEW_TIME   18.0f   /* s an apple keeps her busy */
#define DETOUR_AFTER 1.0f  /* s held up by something (the car): she goes round it ... */
#define DETOUR_TIME  1.2f  /* ... for this long, off to one side */
#define PANT_RISE    0.1f   /* per s running flat out: out of breath (about 10 s to the full) */
#define PANT_EASE    0.035f /* per s resting: back (half a minute; in the sun slower) */
#define PANT_SUN     0.35f  /* the midday sun alone: this much */

static float rnd(Object *dog) {
    unsigned *r = &dog->ai.rng;
    if (!*r) *r = 0x51ed270bu;
    *r = *r * 1664525u + 1013904223u;
    return (float)(*r >> 8) / 16777216.0f;
}

static float flatDist(Vec3 a, Vec3 b) {
    return sqrtf((a.x - b.x) * (a.x - b.x) + (a.z - b.z) * (a.z - b.z));
}

static Object *findType(Object *const *objects, int count, ObjectType type) {
    for (int i = 0; i < count; i++)
        if (objects[i]->type == type) return objects[i];
    return NULL;
}

/* Someone close holding a sausage. */
static Object *sausageHolder(const Object *dog, Object *const *objects, int count) {
    for (int i = 0; i < count; i++) {
        Object *o = objects[i];
        if (!o->is_character || flatDist(o->position, dog->position) > BEG_RANGE) continue;
        for (int h = 0; h < 2; h++)
            if (o->hands.item[h] && o->hands.item[h]->type == OBJ_SAUSAGE) return o;
    }
    return NULL;
}

static void bark(Object *dog, const char *what);

#define PLAY_NEAR   20.0f  /* m: you this near ... */
#define PLAY_CHANCE 0.015f /* ... now and then (a second) she fetches one of her toys and brings it to you */
#define TOY_SEES    15.0f

/* One of her toys lying near. */
static Object *toyNear(const Object *dog, Object *const *objects, int count) {
    Object *best = NULL;
    float best_d = TOY_SEES;
    for (int i = 0; i < count; i++) {
        Object *o = objects[i];
        if (o->type != OBJ_TOY || o->is_held) continue;
        float d = flatDist(o->position, dog->position);
        if (d < best_d) { best = o; best_d = d; }
    }
    return best;
}

/* Something for her: thrown (small enough to carry), or a sausage lying on the ground. */
static Object *somethingToFetch(const Object *dog, Object *const *objects, int count) {
    Object *best = NULL;
    float best_d = 1e9f;
    for (int i = 0; i < count; i++) {
        Object *o = objects[i];
        if (o->is_held || o->broken || o->weight > FETCH_KG || o->collider_count == 0 || o->on_grill) continue;
        float d = flatDist(o->position, dog->position);
        bool thrown = o->thrown_by && o->thrown_by != dog && d < FETCH_SEES;
        bool food = o->type == OBJ_SAUSAGE && !o->thrown_by && o->is_grounded && d < FOOD_SMELLS;
        if ((thrown || food) && d < best_d) { best = o; best_d = d; }
    }
    return best;
}

/* In her mouth: in front of her head. */
static Vec3 mouth(const Object *dog) {
    return vec3_add(dog->position, vec3_add(vec3_scale(Object_facing(dog), 0.5f), vec3(0.0f, 0.27f - 0.09f * dog->dog.lie, 0.0f)));
}

static void letGo(Object *dog) {
    Object *o = dog->dog.carry;
    if (!o) return;
    o->is_held = false;
    o->velocity = vec3_zero();
    o->position = vec3_add(mouth(dog), vec3(0.0f, -0.12f, 0.0f));
    Object_syncColliders(o);
    dog->dog.carry = NULL;
    dog->dog.from = NULL;
}

/* Got it: a sausage goes down at once, an apple she keeps, the rest she brings back. */
static void takeIt(Object *dog, Object *o, bool in_air) {
    dog->dog.fetch = NULL;
    dog->dog.wag = 3.0f;
    if (o->type == OBJ_SAUSAGE) {
        Object_useUp(o);
        bark(dog, in_air ? "*snap* *gulp*" : "*gulp*");
        dog->dog.mode = DOG_WANDER;
        return;
    }
    dog->dog.from = o->thrown_by;
    dog->dog.carry = o;
    o->is_held = true;
    o->thrown_by = NULL;
    o->velocity = vec3_zero();
    dog->dog.chew = 0.0f;
    if (in_air) bark(dog, "*snap!*");
    dog->dog.mode = o->type == OBJ_APPLE ? DOG_KEEP : DOG_BRING;
    if (o->type == OBJ_APPLE) { /* (off with it, away from people) */
        float a = rnd(dog) * 2.0f * PI_F;
        dog->dog.goal = vec3_add(dog->position, vec3(cosf(a) * 4.0f, 0.0f, sinf(a) * 4.0f));
    }
}

/* Turns towards dir and moves that way (smooth turns). */
static void trot(Object *dog, Vec3 dir, float speed, float dt) {
    dir.y = 0.0f;
    float len = vec3_length(dir);
    if (len < 1e-3f) return;
    float want = atan2f(dir.z, dir.x) + PI_F / 2.0f, now = dog->rotation.y;
    float diff = atan2f(sinf(want - now), cosf(want - now));
    dog->rotation.y = now + clampf(diff, -TURN_RATE * dt, TURN_RATE * dt);
    Object_syncColliders(dog);
    dog->movement = vec3_scale(Object_facing(dog), speed * (fabsf(diff) > 1.2f ? 0.3f : 1.0f));
}

static void bark(Object *dog, const char *what) {
    Ai_say(dog, what);
    dog->dog.bark = 0.0f;
}

/* Panting: after a run, and lying about in the midday sun - quick breaths, heard close by */
static void pant(Object *dog, float dt) {
    float warm = 0.0f;
    if (g_world) {
        float h = g_world->hour;
        warm = (smoothstepf(10.5f, 13.0f, h) - smoothstepf(16.5f, 19.0f, h)) * (1.0f - g_world->rain);
    }
    float effort = clampf((dog->dog.speed - 1.8f) / (RUN_SPEED - 1.8f), 0.0f, 1.0f);
    float p = dog->dog.pant + dt * (PANT_RISE * effort - PANT_EASE * (1.0f - effort) * (1.0f - 0.5f * warm));
    dog->dog.pant = clampf(fmaxf(p, PANT_SUN * warm), 0.0f, 1.0f);
    if (dog->dog.pant < 0.15f || dog->dog.carry || !g_world) { dog->dog.pant_t = 0.0f; return; } /* (mouth full: through the nose) */
    float rate = 2.0f + 2.5f * dog->dog.pant; /* breaths a second */
    dog->dog.pant_t += dt * rate;
    if (dog->dog.pant_t >= 1.0f) {
        dog->dog.pant_t -= 1.0f;
        Vec3 head = vec3_add(dog->position, vec3_add(vec3_scale(Object_facing(dog), 0.45f), vec3(0.0f, 0.3f, 0.0f)));
        World_noiseOn(g_world, NOISE_PANT, head, 0.15f + 0.3f * dog->dog.pant, (int)(rate * 10.0f));
    }
}

void Dog_update(Object *dog, Object *const *objects, int count, float dt) {
    if (dog->is_held) return;
    Ai *ai = &dog->ai;
    { /* how fast she really goes: running into something she stops, and goes round it */
        float moved = flatDist(dog->position, dog->dog.was);
        dog->dog.was = dog->position;
        float real = dt > 0.0f && moved < 2.0f ? moved / dt : 0.0f;
        dog->dog.speed += (real - dog->dog.speed) * clampf(10.0f * dt, 0.0f, 1.0f);
        bool held = dog->ground_speed > 1.0f && dog->dog.speed < 0.3f * dog->ground_speed;
        dog->dog.stuck = held ? dog->dog.stuck + dt : fmaxf(0.0f, dog->dog.stuck - 2.0f * dt);
        if (dog->dog.stuck > DETOUR_AFTER && dog->dog.detour == 0.0f) {
            dog->dog.detour = (rnd(dog) < 0.5f ? -1.0f : 1.0f) * DETOUR_TIME;
            dog->dog.stuck = 0.0f;
        }
    }
    ai->speech_time = fmaxf(0.0f, ai->speech_time - dt);
    dog->dog.wag = fmaxf(0.0f, dog->dog.wag - dt);
    dog->dog.bark += dt;
    dog->dog.timer -= dt;
    const Object *player = findType(objects, count, OBJ_PLAYER);
    if (!player) return;
    float to_player = flatDist(player->position, dog->position);
    Object *holder = sausageHolder(dog, objects, count);
    Object *fire = NULL;
    for (int i = 0; i < count; i++)
        if (objects[i]->type == OBJ_FIREPLACE && objects[i]->burning && flatDist(objects[i]->position, dog->position) < FIRE_RANGE)
            fire = objects[i];

    /* Something thrown, or food on the ground: after it */
    Object *carry = dog->dog.carry;
    if (carry && (carry->broken || !carry->is_held)) { dog->dog.carry = carry = NULL; }
    if (carry && dog->dog.mode != DOG_KEEP && dog->dog.mode != DOG_BRING && dog->dog.mode != DOG_COME) dog->dog.mode = DOG_BRING;
    if (!carry) {
        Object *f = dog->dog.fetch;
        if (!f || f->is_held || f->broken) f = somethingToFetch(dog, objects, count);
        dog->dog.fetch = f;
        if (f && dog->dog.mode != DOG_COME) dog->dog.mode = DOG_FETCH;
        else if (!f && dog->dog.mode == DOG_FETCH) dog->dog.mode = DOG_WANDER;
    }

    /* What now */
    int mode = dog->dog.mode;
    if (mode != DOG_COME && mode != DOG_FETCH && mode != DOG_BRING && mode != DOG_KEEP) {
        if (holder) mode = DOG_BEG;
        else if (to_player > FOLLOW_FAR) mode = DOG_FOLLOW;
        else if (mode == DOG_BEG) mode = DOG_WANDER;
        else if (mode == DOG_REST && !fire) mode = DOG_WANDER;
        else if (mode == DOG_WANDER && to_player < PLAY_NEAR && rnd(dog) < PLAY_CHANCE * dt &&
                 (dog->dog.fetch = toyNear(dog, objects, count))) {
            mode = DOG_FETCH; /* (her toy, to bring you: throw it!) */
        }
        else if (mode == DOG_WANDER && fire && dog->dog.timer <= 0.0f && rnd(dog) < 0.5f) {
            mode = DOG_REST;
            float a = rnd(dog) * 2.0f * PI_F;
            dog->dog.goal = vec3_add(fire->position, vec3(cosf(a) * 1.7f, 0.0f, sinf(a) * 1.7f));
        }
    }
    dog->dog.mode = mode;

    Vec3 goal = dog->position;
    float speed = TROT_SPEED;
    bool lie = false;
    switch (mode) {
    case DOG_COME:
        goal = player->position;
        speed = RUN_SPEED;
        if (to_player < 1.2f) {
            dog->dog.mode = DOG_WANDER;
            dog->dog.timer = 4.0f;
            dog->dog.wag = 3.0f;
            bark(dog, "Woof!");
        }
        break;
    case DOG_FOLLOW:
        goal = vec3_sub(player->position, vec3_scale(Object_facing(player), 1.8f));
        speed = to_player > 15.0f ? RUN_SPEED : 2.4f;
        if (flatDist(goal, dog->position) < 1.0f) { dog->dog.mode = DOG_WANDER; dog->dog.timer = 2.0f; }
        break;
    case DOG_BEG: {
        Vec3 away = vec3_sub(dog->position, holder->position);
        away.y = 0.0f;
        float d = vec3_length(away);
        goal = d > 1e-3f ? vec3_add(holder->position, vec3_scale(away, 0.75f / d)) : holder->position;
        if (flatDist(goal, dog->position) < 0.35f) {
            dog->dog.wag = fmaxf(dog->dog.wag, 1.0f);
            if (dog->dog.bark > 6.0f && rnd(dog) < 0.02f) bark(dog, "Woof? Woof!");
        }
        break;
    }
    case DOG_REST:
        goal = dog->dog.goal;
        lie = flatDist(goal, dog->position) < 0.5f;
        break;
    case DOG_FETCH: { /* after it: where it's going (in the air), or where it lies */
        Object *f = dog->dog.fetch;
        Vec3 at = Object_colliderCenter(f, 0);
        goal = vec3_add(at, f->thrown_by ? vec3_scale(vec3(f->velocity.x, 0.0f, f->velocity.z), 0.35f) : vec3_zero());
        goal = vec3_sub(goal, vec3_scale(Object_facing(dog), 0.4f)); /* (her mouth to it, not her feet) */
        speed = RUN_SPEED;
        float d = flatDist(at, mouth(dog)), over = Object_bottom(f) - dog->position.y;
        if (d < 0.4f) {
            if (f->thrown_by && over > 0.08f) { /* still in the air: snap at it */
                if (over < CATCH_HIGH && dog->dog.timer <= 0.0f) {
                    if (rnd(dog) < CATCH_CHANCE) takeIt(dog, f, true);
                    else { dog->dog.timer = 0.5f; bark(dog, "*snap* ...missed"); }
                }
            } else if (over < 0.3f) {
                takeIt(dog, f, false);
            }
        }
        break;
    }
    case DOG_BRING: { /* a stick (or so) back to whoever threw it, at their feet */
        const Object *to = dog->dog.from ? dog->dog.from : player;
        Vec3 away = vec3_sub(dog->position, to->position);
        away.y = 0.0f;
        float d = vec3_length(away);
        goal = d > 1e-3f ? vec3_add(to->position, vec3_scale(away, 0.8f / d)) : to->position;
        speed = RUN_SPEED * 0.8f;
        if (flatDist(goal, dog->position) < 0.4f) {
            letGo(dog);
            dog->dog.mode = DOG_WANDER;
            dog->dog.timer = 3.0f;
            dog->dog.wag = 4.0f;
            bark(dog, "Woof! Woof!");
        }
        break;
    }
    case DOG_KEEP: /* an apple: off with it, down, and eaten bit by bit */
        goal = dog->dog.goal;
        lie = flatDist(goal, dog->position) < 0.5f;
        if (lie) {
            dog->dog.chew += dt;
            if (dog->dog.bark > 5.0f && rnd(dog) < 0.05f) bark(dog, "*crunch crunch*");
            if (dog->dog.chew > CHEW_TIME) {
                Object *a = dog->dog.carry;
                dog->dog.carry = NULL;
                if (a) Object_useUp(a);
                dog->dog.mode = DOG_WANDER;
                dog->dog.timer = 2.0f;
            }
        }
        break;
    default: /* sniffing around near you */
        if (dog->dog.timer <= 0.0f) {
            float a = rnd(dog) * 2.0f * PI_F, r = 1.5f + 4.0f * rnd(dog);
            dog->dog.goal = vec3_add(player->position, vec3(cosf(a) * r, 0.0f, sinf(a) * r));
            dog->dog.timer = 4.0f + 6.0f * rnd(dog);
        }
        goal = dog->dog.goal;
        if (dog->dog.bark > 40.0f && rnd(dog) < 0.01f) bark(dog, rnd(dog) < 0.5f ? "Woof!" : "*sniff sniff*");
        break;
    }

    Vec3 to = vec3_sub(goal, dog->position);
    to.y = 0.0f;
    if (dog->dog.detour != 0.0f) { /* round what's in the way: off to the side, a bit on */
        float way = dog->dog.detour > 0.0f ? 1.0f : -1.0f, len = vec3_length(to);
        Vec3 ahead = len > 1e-3f ? vec3_scale(to, 1.0f / len) : Object_facing(dog);
        trot(dog, vec3_add(vec3(-ahead.z * way, 0.0f, ahead.x * way), vec3_scale(ahead, 0.3f)), TROT_SPEED, dt);
        dog->dog.detour = way * fmaxf(0.0f, fabsf(dog->dog.detour) - dt);
    }
    else if (!lie && vec3_length(to) > 0.35f) trot(dog, to, speed, dt);
    else if (mode == DOG_BEG && holder) trot(dog, vec3_scale(vec3_sub(holder->position, dog->position), 0.001f), 0.0f, dt);
    dog->dog.lie = clampf(dog->dog.lie + (lie ? dt : -2.0f * dt), 0.0f, 1.0f);
    if (dog->dog.carry) { /* in her mouth */
        Object *c = dog->dog.carry;
        c->position = mouth(dog);
        c->rotation = vec3(PI_F * 0.5f, dog->rotation.y + PI_F * 0.5f, 0.0f); /* (crosswise) */
        Object_syncColliders(c);
    }
    dog->dog.phase += dog->dog.speed * dt * 11.0f; /* (held up by something: her legs stop too) */
    pant(dog, dt);
}

void Dog_call(Object *dog, const Object *player) {
    (void)player;
    dog->dog.mode = DOG_COME;
    dog->dog.wag = 2.0f;
    bark(dog, "Woof!");
}

void Dog_pet(Object *dog) {
    dog->dog.wag = 4.0f;
    dog->dog.timer = 5.0f;
    bark(dog, rnd(dog) < 0.5f ? "*happy whine*" : "*licks your hand*");
}

/* Drawn like the people (a painted figure, see figure.frag): her body as a skeleton.
   Figure space: x right, y up, z forward, from her feet. */
#define DOG_COAT ((Vec3){ 0.55f, 0.30f, 0.14f })
#define DOG_DARK ((Vec3){ 0.33f, 0.17f, 0.08f })
#define DOG_LEG  (DOG_HIP_Y + 0.03f)

void Dog_draw(Renderer *r, const Object *dog) {
    FigurePose pose;
    memset(&pose, 0, sizeof(pose));
    pose.position = dog->position;
    pose.facing = Object_facing(dog);
    pose.look.skin = DOG_COAT;
    pose.look.hair = DOG_DARK;
    pose.look.height = 0.4f;
    Skeleton *sk = &pose.skeleton;
    sk->species = FIGURE_DOG;
    sk->cig_glow = -1.0f;

    const float lie = smoothstepf(0.0f, 1.0f, dog->dog.lie), dy = -0.09f * lie;
    const float moving = clampf(dog->dog.speed / TROT_SPEED, 0.0f, 1.0f);
    const float t = r->boil_time, pant = dog->dog.carry ? 0.0f : dog->dog.pant;
    const float breathe = 0.004f * sinf(t * 5.0f) + 0.012f * pant * sinf(2.0f * PI_F * dog->dog.pant_t); /* (panting: the chest going quick, with each breath) */
    const float bob = 0.008f * moving * fabsf(sinf(dog->dog.phase));
    sk->joints[JOINT_PELVIS] = vec3(0.0f, 0.21f + dy + bob, -0.18f);
    sk->joints[JOINT_CHEST]  = vec3(0.0f, 0.22f + dy + bob + breathe, 0.17f);
    sk->joints[JOINT_NECK]   = vec3(0.0f, 0.29f + dy + bob, 0.27f);
    const float sniff = dog->dog.mode == DOG_WANDER && moving < 0.6f && lie < 0.1f ? 0.5f : 0.0f; /* (nose down, sniffing about) */
    sk->joints[JOINT_HEAD]   = vec3(0.0f, 0.33f + dy + bob - 0.06f * lie - 0.08f * sniff, 0.35f);
    sk->head_forward = vec3_normalize(vec3(0.0f, -0.12f - sniff, 1.0f));
    sk->head_up = vec3_normalize(vec3(0.0f, 1.0f, 0.12f + sniff));
    sk->chest_forward = vec3(0.0f, 0.0f, 1.0f);

    /* Legs: trotting, diagonal pairs together; lying, the front legs forward, the hind ones back */
    static const int SLOT[4][4] = {
        { JOINT_SHOULDER_L, JOINT_ELBOW_L, JOINT_WRIST_L, JOINT_HAND_L }, { JOINT_SHOULDER_R, JOINT_ELBOW_R, JOINT_WRIST_R, JOINT_HAND_R },
        { JOINT_HIP_L, JOINT_KNEE_L, JOINT_ANKLE_L, JOINT_TOE_L },         { JOINT_HIP_R, JOINT_KNEE_R, JOINT_ANKLE_R, JOINT_TOE_R },
    };
    for (int k = 0; k < 4; k++) {
        bool front = k < 2;
        float side = k % 2 ? 1.0f : -1.0f;
        Vec3 hip = vec3(side * DOG_HIP_X, DOG_HIP_Y + 0.05f + dy + bob, front ? DOG_HIP_Z - 0.04f : -DOG_HIP_Z + 0.02f);
        float swing = sinf(dog->dog.phase + (front ? 0.0f : PI_F) + (side > 0.0f ? PI_F : 0.0f)) * 0.6f * moving;
        swing = swing * (1.0f - lie) + (front ? -1.35f : 1.35f) * lie;
        Vec3 paw = vec3_add(hip, vec3(0.0f, -DOG_LEG * cosf(swing), -DOG_LEG * sinf(swing)));
        if (lie < 0.5f) paw.y = fmaxf(paw.y, 0.022f);
        Vec3 knee = vec3_add(vec3_scale(vec3_add(hip, paw), 0.5f), vec3(0.0f, 0.0f, front ? -0.012f : 0.022f));
        sk->joints[SLOT[k][0]] = hip;
        sk->joints[SLOT[k][1]] = knee;
        sk->joints[SLOT[k][2]] = paw;
        sk->joints[SLOT[k][3]] = vec3_add(paw, vec3(0.0f, -0.004f, 0.035f));
    }
    float wag = sinf(t * 20.0f) * (0.15f + 0.45f * fminf(dog->dog.wag, 1.0f));
    sk->tail_a = vec3(0.0f, DOG_TAIL_Y + dy + bob, -DOG_TAIL_Z + 0.03f);
    sk->tail_b = vec3_add(sk->tail_a, vec3(0.17f * sinf(wag), 0.07f, -0.17f * cosf(wag)));
    Renderer_drawFigure(r, &pose);
}

void Dog_shutdown(void) {
}