/* collide.c
 *
 * Collision code
 *
 * Created: 9/7/97
 */

#include "brender.h"

#include "chaos.h"

static c_chobj *collidewithtmp, *collidefromtmp;
static br_vector3 *pointtmp1, *pointtmp2;
static br_scalar rangetmp;
static br_vector4 *planetmp;
static br_actor *thistmp;
static br_vector3 intersection, poitmp;
static br_uint_32 flagstmp, coltype;
static br_vector3 slidepoint;

br_scalar c_CompareVertexWithPlane(br_vector3 *v, br_vector4 *p)
{
    return v->v[0]*p->v[0]+v->v[1]*p->v[1]+v->v[2]*p->v[2]-p->v[3];
}

br_scalar c_CompareVertexWithTree(br_vector3 *v, c_bsp *root)
{
    br_scalar diff = c_CompareVertexWithPlane(v, &root->plane) - rangetmp;

    if(diff < 0) {
        if(root->in)
            return c_CompareVertexWithTree(v, root->in);
        else
            return diff;
    } else {
        if(root->out)
            return c_CompareVertexWithTree(v, root->out);
        else
            return diff;
    }
}

void c_IntersectLineWithPlane(br_vector3 *v3, br_vector3 *v1, br_vector3 *v2,
                              br_scalar diff1, br_scalar diff2)
{
    v3->v[0] = v2->v[0]+
        BR_DIV(BR_MUL((v1->v[0]-v2->v[0]), diff2), (diff2-diff1));
    v3->v[1] = v2->v[1]+
        BR_DIV(BR_MUL((v1->v[1]-v2->v[1]), diff2), (diff2-diff1));
    v3->v[2] = v2->v[2]+
        BR_DIV(BR_MUL((v1->v[2]-v2->v[2]), diff2), (diff2-diff1));
}

br_scalar c_CompareLineWithTree(br_vector4 **plane, br_vector3 *v1,
                                br_vector3 *v2, c_bsp *root)
{
    br_scalar diff1 = c_CompareVertexWithPlane(v1, &root->plane) - rangetmp,
              diff2 = c_CompareVertexWithPlane(v2, &root->plane) - rangetmp,
              diff3;
    br_vector3 v3;

    if(diff1 < 0 && diff2 < 0) {
        if(root->in)
            return c_CompareLineWithTree(plane, v1, v2, root->in);
        else
            return diff1;
    } else if(diff1 >= 0 && diff2 >= 0) {
        if(root->out)
            return c_CompareLineWithTree(plane, v1, v2, root->out);
        else
            return diff1;
    } else {
        c_IntersectLineWithPlane(&v3, v1, v2, diff1, diff2);

        if(diff1 < 0){
            if(root->in)
                diff3 = c_CompareLineWithTree(plane, v1, &v3, root->in);
            else
                diff3 = diff1;
            if(diff3 == 0)
                return diff3;
            if(root->out)
                diff3 = c_CompareLineWithTree(plane, &v3, v2, root->out);
            else
                diff3 = diff2;
            return diff3;
        } else {
            if(root->out)
                diff3 = c_CompareLineWithTree(plane, v1, &v3, root->out);
            else
                diff3 = diff1;
            if(diff3 == 0)
                return diff3;
            if(diff3 < 0){
                *plane = &root->plane;
                c_IntersectLineWithPlane(&intersection, v1, v2, diff1 - BR_SCALAR(0.01), diff2 - BR_SCALAR(0.01));
                return 0;
            }
            if(root->in)
                diff3 = c_CompareLineWithTree(plane, &v3, v2, root->in);
            else
                diff3 = diff2;
            if(diff3 < 0){
                *plane = &root->plane;
                c_IntersectLineWithPlane(&intersection, v1, v2, diff1 - BR_SCALAR(0.01), diff2 - BR_SCALAR(0.01));
                return 0;
            }
            if(diff3 == 0){
                if(root->in){
                    if(c_CompareVertexWithTree(&v3, root->in) < 0){
                        *plane = &root->plane;
                        c_IntersectLineWithPlane(&intersection, v1, v2, diff1 - BR_SCALAR(0.01), diff2 - BR_SCALAR(0.01));
                        return 0;
                    }
                } else {
                    *plane = &root->plane;
                    c_IntersectLineWithPlane(&intersection, v1, v2, diff1 - BR_SCALAR(0.01), diff2 - BR_SCALAR(0.01));
                    return 0;
                }
            }
            return diff3;
        }
    }
}

static br_uint_32 BR_CALLBACK TestCollideVertex(br_actor *a, void *root)
{
    br_matrix34 tmpmat;
    br_scalar diff;

    if(!a->model->user || a == thistmp)
        return 0;

    if(a->parent == root &&
       a->t.t.mat.m[0][0] == BR_SCALAR(1) &&
       a->t.t.mat.m[1][1] == BR_SCALAR(1) &&
       a->t.t.mat.m[2][2] == BR_SCALAR(1))
        BrMatrix34Translate(&tmpmat, -a->t.t.mat.m[3][0],
            -a->t.t.mat.m[3][1], -a->t.t.mat.m[3][2]);
    else
        BrActorToActorMatrix34(&tmpmat, root, a);
    BrMatrix34PreTranslate(&tmpmat, pointtmp1->v[0],
        pointtmp1->v[1], pointtmp1->v[2]);

    if((tmpmat.m[3][0] < (a->model->bounds.min.v[0]-rangetmp)) ||
       (tmpmat.m[3][1] < (a->model->bounds.min.v[1]-rangetmp)) ||
       (tmpmat.m[3][2] < (a->model->bounds.min.v[2]-rangetmp)) ||
       (tmpmat.m[3][0] > (a->model->bounds.max.v[0]+rangetmp)) ||
       (tmpmat.m[3][1] > (a->model->bounds.max.v[1]+rangetmp)) ||
       (tmpmat.m[3][2] > (a->model->bounds.max.v[2]+rangetmp)))
        return 0;

    diff = c_CompareVertexWithTree((br_vector3 *)&tmpmat.m[3],
        ((c_mci *)a->model->user)->bsp);
    if(diff <= 0)
        collidewithtmp = a->user;

    return 0;
}

c_chobj *c_CollidePointWithObjects(c_chobj *object, br_scalar range)
{
    int x, y;
    br_int_32 xpos, ypos;

    thistmp = object->skeleton->actors[0];
    pointtmp1 = (br_vector3 *)&object->skeleton->actors[0]->t.t.mat.m[3];
    rangetmp = range;
    collidewithtmp = NULL;
    collidefromtmp = object;

    xpos = BrScalarToInt(BR_DIV(pointtmp1->v[0],
        C_MAPZONESIZE*game->map->scale))-1;
    ypos = BrScalarToInt(BR_DIV(pointtmp1->v[2],
        C_MAPZONESIZE*game->map->scale))-1;

    if(xpos < 0)
        xpos = 0;
    if(ypos < 0)
        ypos = 0;
    if(xpos >= game->map->width/C_MAPZONESIZE)
        xpos = game->map->width/C_MAPZONESIZE-1;
    if(ypos >= game->map->height/C_MAPZONESIZE)
        ypos = game->map->height/C_MAPZONESIZE-1;

    for(y = ypos; y < ypos+3; y++)
        for(x = xpos; x < xpos+3; x++)
            BrActorEnum(&game->map->zones[x+y*(game->map->width/C_MAPZONESIZE)],
                TestCollideVertex,
                (void *)&game->map->zones[x+y*(game->map->width/C_MAPZONESIZE)]);

    return collidewithtmp;
}

static br_uint_32 BR_CALLBACK TestCollideLine(br_actor *a, void *root)
{
    br_matrix34 tmpmat1, tmpmat2, invmat;
    br_scalar diff;

    if(!a->model->user || a == thistmp)
        return 0;

    if(a->parent == root &&
       a->t.t.mat.m[0][0] == BR_SCALAR(1) &&
       a->t.t.mat.m[1][1] == BR_SCALAR(1) &&
       a->t.t.mat.m[2][2] == BR_SCALAR(1))
        BrMatrix34Translate(&tmpmat1, -a->t.t.mat.m[3][0],
            -a->t.t.mat.m[3][1], -a->t.t.mat.m[3][2]);
    else
        BrActorToActorMatrix34(&tmpmat1, root, a);
    BrMatrix34Inverse(&invmat, &tmpmat1);
    BrMatrix34Copy(&tmpmat2, &tmpmat1);
    BrMatrix34PreTranslate(&tmpmat1, pointtmp1->v[0], pointtmp1->v[1],
        pointtmp1->v[2]);
    BrMatrix34PreTranslate(&tmpmat2, pointtmp2->v[0], pointtmp2->v[1],
        pointtmp2->v[2]);

    if(((tmpmat1.m[3][0] < (a->model->bounds.min.v[0]-rangetmp))&&
        (tmpmat2.m[3][0] < (a->model->bounds.min.v[0]-rangetmp)))||
       ((tmpmat1.m[3][1] < (a->model->bounds.min.v[1]-rangetmp))&&
        (tmpmat2.m[3][1] < (a->model->bounds.min.v[1]-rangetmp)))||
       ((tmpmat1.m[3][2] < (a->model->bounds.min.v[2]-rangetmp))&&
        (tmpmat2.m[3][2] < (a->model->bounds.min.v[2]-rangetmp)))||
       ((tmpmat1.m[3][0] > (a->model->bounds.max.v[0]+rangetmp))&&
        (tmpmat2.m[3][0] > (a->model->bounds.max.v[0]+rangetmp)))||
       ((tmpmat1.m[3][1] > (a->model->bounds.max.v[1]+rangetmp))&&
        (tmpmat2.m[3][1] > (a->model->bounds.max.v[1]+rangetmp)))||
       ((tmpmat1.m[3][2] > (a->model->bounds.max.v[2]+rangetmp))&&
        (tmpmat2.m[3][2] > (a->model->bounds.max.v[2]+rangetmp))))
        return 0;

    if(!(flagstmp & ((c_mci *)a->model->user)->flags)){
        collidewithtmp = a->user;
        coltype = 0;
        return 0;
    }

    diff = c_CompareLineWithTree(&planetmp,
        (br_vector3 *)&tmpmat1.m[3], (br_vector3 *)&tmpmat2.m[3],
        ((c_mci *)a->model->user)->bsp);
    if(diff == 0){
        br_vector3 tmppos;

        collidewithtmp = a->user;
        BrMatrix34ApplyP(&poitmp, &intersection, &invmat);

        diff = c_CompareVertexWithPlane((br_vector3 *)&tmpmat2.m[3],
            planetmp) - rangetmp - BR_SCALAR(0.01);
        tmppos.v[0] = BR_SUB(tmpmat2.m[3][0],
            BR_MUL(planetmp->v[0], diff));
        tmppos.v[1] = BR_SUB(tmpmat2.m[3][1],
            BR_MUL(planetmp->v[1], diff));
        tmppos.v[2] = BR_SUB(tmpmat2.m[3][2],
            BR_MUL(planetmp->v[2], diff));

        BrMatrix34ApplyP(&slidepoint, &tmppos, &invmat);

        coltype = 1;
    }

    return 0;
}

c_chobj *c_CollideLineWithObjects(br_vector3 **poi, c_chobj *object,
                                  br_scalar range, br_uint_32 flags)
{
    int x, y;
    br_int_32 xpos, ypos;

    flagstmp = flags;
    thistmp = object->skeleton->actors[0];
    pointtmp1 = (br_vector3 *)&object->phys_lastx;
    pointtmp2 = (br_vector3 *)&object->skeleton->actors[0]->t.t.mat.m[3];
    rangetmp = 0;//range;
    collidewithtmp = NULL;
    collidefromtmp = object;

    xpos = BrScalarToInt(BR_DIV(pointtmp1->v[0],
        C_MAPZONESIZE*game->map->scale))-1;
    ypos = BrScalarToInt(BR_DIV(pointtmp1->v[2],
        C_MAPZONESIZE*game->map->scale))-1;

    if(xpos < 0)
        xpos = 0;
    if(ypos < 0)
        ypos = 0;
    if(xpos >= game->map->width/C_MAPZONESIZE)
        xpos = game->map->width/C_MAPZONESIZE-1;
    if(ypos >= game->map->height/C_MAPZONESIZE)
        ypos = game->map->height/C_MAPZONESIZE-1;

    for(y = ypos; y < ypos+3; y++)
        for(x = xpos; x < xpos+3; x++)
            BrActorEnum(&game->map->zones[x+y*(game->map->width/C_MAPZONESIZE)],
                TestCollideLine,
                (void *)&game->map->zones[x+y*(game->map->width/C_MAPZONESIZE)]);

    *poi = &poitmp;

    return collidewithtmp;
}

void c_PlaneSlide(c_chobj *object)
{
    if(collidewithtmp){
        object->skeleton->actors[0]->t.t.mat.m[3][0] = slidepoint.v[0];
        object->skeleton->actors[0]->t.t.mat.m[3][1] = slidepoint.v[1];
        object->skeleton->actors[0]->t.t.mat.m[3][2] = slidepoint.v[2];
    }
}

void c_PlaneClip(c_chobj *object)
{
    if(collidewithtmp){
        object->skeleton->actors[0]->t.t.mat.m[3][0] = poitmp.v[0];
        object->skeleton->actors[0]->t.t.mat.m[3][1] = poitmp.v[1];
        object->skeleton->actors[0]->t.t.mat.m[3][2] = poitmp.v[2];
    }
}

void c_GetCollisionState(c_chobj **with, c_chobj **from, br_vector4 **plane)
{
    *with = collidewithtmp;
    *from = collidefromtmp;
    *plane = planetmp;
}

