#include "movenode.qh"

#include "pathlib.qh"
#include "utility.qh"

.vector	pos1, pos2;

vector pathlib_wateroutnode(entity this, vector start, vector end, float doedge)
{
    vector surface;

    pathlib_movenode_goodnode = 0;

    end.x = fsnap(end.x, pathlib_gridsize);
    end.y = fsnap(end.y, pathlib_gridsize);

    traceline(end + ('0 0 0.25' * pathlib_gridsize),end - ('0 0 1' * pathlib_gridsize),MOVE_WORLDONLY,this);
    end = trace_endpos;

    if (!(pointcontents(end - '0 0 1') == CONTENT_SOLID))
        return end;

    for(surface = start ; surface.z < (end.z + 32); ++surface.z)
    {
        if(pointcontents(surface) == CONTENT_EMPTY)
            break;
    }

    if(pointcontents(surface + '0 0 1') != CONTENT_EMPTY)
        return end;

    tracebox(start + '0 0 64', movenode_boxmin,movenode_boxmax, end + '0 0 64', MOVE_WORLDONLY, this);
    if(trace_fraction == 1)
        pathlib_movenode_goodnode = 1;

    if(fabs(surface.z - end.z) > 32)
        pathlib_movenode_goodnode = 0;

    return end;
}

vector pathlib_swimnode(entity this, vector start, vector end, float doedge)
{
    pathlib_movenode_goodnode = 0;

    if(pointcontents(start) != CONTENT_WATER)
        return end;

    end.x = fsnap(end.x, pathlib_gridsize);
    end.y = fsnap(end.y, pathlib_gridsize);

    if(pointcontents(end) == CONTENT_EMPTY)
        return pathlib_wateroutnode(this, start, end, doedge);

    tracebox(start, movenode_boxmin,movenode_boxmax, end, MOVE_WORLDONLY, this);
    if(trace_fraction == 1)
        pathlib_movenode_goodnode = 1;

    return end;
}

vector pathlib_flynode(entity this, vector start, vector end, float doedge)
{
    pathlib_movenode_goodnode = 0;

    end.x = fsnap(end.x, pathlib_gridsize);
    end.y = fsnap(end.y, pathlib_gridsize);

    tracebox(start, movenode_boxmin,movenode_boxmax, end, MOVE_WORLDONLY, this);
    if(trace_fraction == 1)
        pathlib_movenode_goodnode = 1;

    return end;
}

void a_think(entity this)
{
    te_lightning1(this,this.origin, this.pos1);
    if(this.cnt < time)
        remove(this);
    else
        this.nextthink = time + 0.2;
}

vector pathlib_walknode(entity this, vector start, vector end, float doedge)
{
    vector direction,point,last_point,s,e;
    float steps, distance, i;

    LOG_TRACE("Walking node from ", vtos(start), " to ", vtos(end), "\n");

    pathlib_movenode_goodnode = 0;

    end.x = fsnap(end.x,pathlib_gridsize);
    end.y = fsnap(end.y,pathlib_gridsize);
    start.x = fsnap(start.x,pathlib_gridsize);
    start.y = fsnap(start.y,pathlib_gridsize);

    // Find the floor
    traceline(start + movenode_stepup, start - movenode_maxdrop, MOVE_WORLDONLY, this);
    if(trace_fraction == 1.0)
    {
        entity a;
        a = spawn();
        setthink(a, a_think);
        a.nextthink = time;
        setorigin(a,start + movenode_stepup);
        a.pos1 = trace_endpos;
        //start - movenode_maxdrop
        a.cnt = time + 10;

        LOG_TRACE("I cant walk on air!\n");
        return trace_endpos;
    }

    start = trace_endpos;

    // Find the direcion, without Z
    s   = start; e   = end;
    //e_z = 0; s_z = 0;
    direction = normalize(e - s);

    distance  = vlen(start - end);
    steps     = rint(distance / movenode_stepsize);

    last_point = start;
    for(i = 1; i < steps; ++i)
    {
        point = last_point + (direction * movenode_stepsize);
        traceline(point + movenode_stepup,point - movenode_maxdrop,MOVE_WORLDONLY,this);
        if(trace_fraction == 1.0)
            return trace_endpos;

        last_point = trace_endpos;
    }

    point = last_point + (direction * movenode_stepsize);
    point.x = fsnap(point.x,pathlib_gridsize);
    point.y = fsnap(point.y,pathlib_gridsize);

    //dprint("end_x:  ",ftos(end_x),  "  end_y:  ",ftos(end_y),"\n");
    //dprint("point_x:",ftos(point_x),"  point_y:",ftos(point_y),"\n\n");

    traceline(point + movenode_stepup, point - movenode_maxdrop,MOVE_WORLDONLY,this);
    if(trace_fraction == 1.0)
        return trace_endpos;

    last_point = trace_endpos;

    tracebox(start + movenode_boxup, movenode_boxmin,movenode_boxmax, last_point + movenode_boxup, MOVE_WORLDONLY, this);
    if(trace_fraction != 1.0)
        return trace_endpos;

    pathlib_movenode_goodnode = 1;
    return last_point;
}