AutoPilot dbg

#include <a_samp>
#include <RouteConnector>
////////////////////////////////////////////////////////////////////////////////
new id[MAX_PLAYERS] = {-1,...};
new CurrTarget[MAX_PLAYERS] = {0,...};
new Timer[MAX_PLAYERS] = {-1,...};
new interval = 75;
new string[128];
new Float:pos[2][3];
////////////////////////////////////////////////////////////////////////////////
public OnPlayerCommandText(playerid,cmdtext[])
{
    if(!strcmp(cmdtext,"/car",true))
    {
        new Float:V[4];
        GetPlayerPos(playerid,V[0],V[1],V[2]);
        CreateVehicle(562,V[0],V[1],V[2],0.0,-1,-1,9999999);
        return 1;
    }
    if(!strcmp(cmdtext,"/ap",true))
    {
        if(id[playerid] == -1)
        CalculatePath(23833,16734,playerid);
        return 1;
    }
    if(!strcmp(cmdtext,"/debug",true))
    {
        if(id[playerid] == -1)
        CalculatePath(23833,16734,playerid+1000);
        return 1;
    }
    if(!strcmp(cmdtext,"/i",true))
    {
        interval +=25;
        format(string,128,"Interval: %d",interval);
        SendClientMessage(playerid,-1,string);
        return 1;
    }
    if(!strcmp(cmdtext,"/d",true))
    {
        interval-=25;
        format(string,128,"Interval: %d",interval);
        SendClientMessage(playerid,-1,string);
        return 1;
    }
    if(!strcmp(cmdtext,"/e",true))
    {
        if(Timer[playerid] != (-1))
        {
            KillTimer(Timer[playerid]);
            Timer[playerid] = -1;
            DeleteArray(id[playerid]);
            id[playerid] = -1;
            CurrTarget[playerid] = 0;
        }
        CallRemoteFunction("OnPlayerCommandText","is",playerid,"/lv");
        return 1;
    }
    return 0;
}
////////////////////////////////////////////////////////////////////////////////
public GPS_WhenRouteIsCalculated(routeid,node_id_array[],amount_of_nodes,distance)
{
    format(string,128,"Auto Pilot has been turned on captain! Nodes: %d",amount_of_nodes);
    if(routeid > 999)
    {
        id[routeid-1000] = StoreRouteArray(amount_of_nodes,node_id_array);
        Timer[routeid-1000] = SetTimerEx("AutoPilotStep",interval,1,"i",routeid-1000);
        SendClientMessage(routeid-1000,-1,string);
    }
    else
    {
        id[routeid] = StoreRouteArray(amount_of_nodes,node_id_array);
        Timer[routeid] = SetTimerEx("AutoPilot",interval,1,"i",routeid);
        SendClientMessage(routeid,-1,string);
    }
    return 1;
}

public OnPlayerClosestNodeIDChange(playerid,old_NodeID,new_NodeID)
{
//  SetVehicleVelocity(GetPlayerVehicleID(playerid),0.0,0.0,0.0);
    return 1;
}

forward AutoPilotStep(playerid);
public AutoPilotStep(playerid)
{
    if(IsPlayerInAnyVehicle(playerid))
    {
        new vehicleid = GetPlayerVehicleID(playerid);
        SetVehicleHealth(vehicleid,1000.0);
        new amount;
        GetNodePos(GetRouteAtPos(id[playerid],CurrTarget[playerid]++,amount),pos[0][0],pos[0][1],pos[0][2]);
        GetNodePos(GetRouteAtPos(id[playerid],(CurrTarget[playerid]+1),amount),pos[1][0],pos[1][1],pos[1][2]);
        SetVehiclePos(vehicleid,pos[0][0],pos[0][1],pos[0][2]+2.0);
        SetVehicleZAngle(vehicleid,atan2VehicleZ(pos[0][0],pos[0][1],pos[1][0],pos[1][1]));
        if(amount <= (CurrTarget[playerid]+1))
        {
            KillTimer(Timer[playerid]);
            Timer[playerid] = -1;
            DeleteArray(id[playerid]);
            id[playerid] = -1;
            CurrTarget[playerid] = 0;
        }
    }
    else
    {
        KillTimer(Timer[playerid]);
        Timer[playerid] = -1;
        DeleteArray(id[playerid]);
        id[playerid] = -1;
        CurrTarget[playerid] = 0;
    }
}

forward AutoPilot(playerid);
public AutoPilot(playerid)
{
    if(IsPlayerInAnyVehicle(playerid))
    {
        new vehicleid = GetPlayerVehicleID(playerid);
        SetVehicleHealth(vehicleid,1000.0);
        if(CurrTarget[playerid] == 0)
        {
            CurrTarget[playerid]++;
            GetNodePos(GetRouteAtPos(id[playerid],CurrTarget[playerid]),pos[0][0],pos[0][1],pos[0][2]);
            SetVehiclePos(vehicleid,pos[0][0],pos[0][1],pos[0][2]+2.0);
            //format(string,128,"NodeID: %d\tX: %.2f Y: %.2f Z: %.2f T: %d",GetRouteAtPos(id[playerid],CurrTarget[playerid]),pos[0][0],pos[0][1],pos[0][2],CurrTarget[playerid]);
            //SendClientMessage(playerid,-1,string);
            return 1;
        }
        new amount;
        new nodeid = GetRouteAtPos(id[playerid],CurrTarget[playerid],amount);
        if((CurrTarget[playerid]+1) >= amount)
        {
            KillTimer(Timer[playerid]);
            Timer[playerid] = -1;
            DeleteArray(id[playerid]);
            id[playerid] = -1;
            CurrTarget[playerid] = 0;
            return 1;
        }
        //GetVehiclePos(vehicleid,pos[0][0],pos[0][1],pos[0][2]);
        GetNodePos(nodeid,pos[1][0],pos[1][1],pos[1][2]);
        //format(string,128,"NodeID: %d\tX: %.2f Y: %.2f Z: %.2f T: %d",nodeid,pos[1][0],pos[1][1],pos[1][2],CurrTarget[playerid]);
        if(IsPlayerInRangeOfPoint(playerid,3.0,pos[1][0],pos[1][1],pos[1][2]))
        {
            CurrTarget[playerid]++;
            return 1;
            //format(string,128,"%s IR",string);
            //GetNodePos(nodeid,pos[1][0],pos[1][1],pos[1][2]);
        }
        //format(string,128,"NodeID: %d\tX: %.2f Y: %.2f Z: %.2f T: %d",nodeid,pos[1][0],pos[1][1],pos[1][2],CurrTarget[playerid]);
        //SendClientMessage(playerid,-1,string);
        //Now lets engage the auto pilot!
        PullVehicleIntoDirection(vehicleid,pos[1][0],pos[1][1],pos[1][2],0.32,playerid);
    }
    else
    {
        KillTimer(Timer[playerid]);
        Timer[playerid] = -1;
        DeleteArray(id[playerid]);
        id[playerid] = -1;
        CurrTarget[playerid] = 0;
    }
    return 1;
}
//--------------------AUTO-PILOT-CORE-FUNCTIONS-------------------------------//
stock PullVehicleIntoDirection(VehicleID,Float:X,Float:Y,Float:Z,Float:Speed,playerid=0)//Or DriveVehicleToDirection.. ShootVehicletodirection.. whatever
{
    #pragma unused Z
    new Float:StartPos[3];
    new Float:u[2];
    GetVehiclePos(VehicleID, StartPos[0],StartPos[1],StartPos[2]);
    SetVehicleZAngle(VehicleID,atan2VehicleZ(StartPos[0],StartPos[1],X,Y));
    u[0] = X-StartPos[0];//deltaX
    u[1] = Y-StartPos[1];//deltaY
    //if(u[0] < 0.01)u[0] = 0.1;//does not fix the problem...
    u[1] = u[1]/u[0];//RC = dY/dX
    if(5.0 > u[1] > -5.0)
    {
        GetVehicleVelocity(VehicleID,StartPos[0],StartPos[1],StartPos[2]);//fix for 'flying car'
        if(u[0] < 0)
        {
            format(string,128,"u[0]: %.4f\tu[1]: %.4f\t ---- Y-S: %.4f",u[0],u[1],Y-StartPos[1]);
            SetVehicleVelocity(VehicleID,(Speed*-1.0),((Speed*u[1])*-1.0),StartPos[2]);
        }
        else
        {
            format(string,128,"u[0]: %.4f\tu[1]: %.4f\t ++++ Y-S: %.4f",u[0],u[1],Y-StartPos[1]);
            SetVehicleVelocity(VehicleID,Speed,Speed*u[1],StartPos[2]);
        }
    }
    else
    {
        format(string,128,"u[0]: %.4f\tu[1]: %.4f\t RRRR Y-S: %.4f",u[0],u[1],Y-StartPos[1]);
    }
    SendClientMessage(playerid,-1,string);
    return 1;
}

forward Float:atan2VehicleZ(Float:Xb,Float:Yb,Float:Xe,Float:Ye);// Dunno how to name it...
stock Float:atan2VehicleZ(Float:Xb,Float:Yb,Float:Xe,Float:Ye)
{
    new Float:a = floatabs(360.0 - atan2( Xe-Xb,Ye-Yb));
    if(360 > a > 180)return a;
    return a-360.0;
}
//----------------------------------------------------------------------------//