//Includes
#include <a_samp>
#include <float>
#include <a_vehicles>

//Using this function in the /cap command
native IsValidVehicle(vehicleid);

//Stores the 3dtextdraw
new td=0;
new Text3D:tdt;


public OnFilterScriptExit()
{
    if(td)
        Delete3DTextLabel(tdt);//unload the textlabel when reloading/ unloading fs
    return 1;
}

//Credits to [HLF]Southclaw
QuatToEulerZXY(Float:quat_x, Float:quat_y, Float:quat_z, Float:quat_w, &Float:x, &Float:y, &Float:z)
{
    x = -asin(2 * ((quat_x * quat_z) + (quat_w * quat_y)));
    y = atan2(2 * ((quat_y * quat_z) + (quat_w * quat_x)), (quat_w * quat_w) - (quat_x * quat_x) - (quat_y * quat_y) + (quat_z * quat_z));
    z = -atan2(2 * ((quat_x * quat_y) + (quat_w * quat_z)), (quat_w * quat_w) + (quat_x * quat_x) - (quat_y * quat_y) - (quat_z * quat_z));
    return 1;
}

//Returns the rotation in degrees of a vehicle
GetVR(vehicleid, &Float:x, &Float:y, &Float:z)
{
    new Float:quat_w,Float:quat_x,Float:quat_y,Float:quat_z;
    GetVehicleRotationQuat(vehicleid,quat_w,quat_x,quat_y,quat_z);
    QuatToEulerZXY(quat_x, quat_y, quat_z, quat_w, x, y, z);
    return 1;
}

//Credits to [HLF]Southclaw
stock GetAttachedObjectPos(
    Float:object_px, Float:object_py, Float:object_pz, Float:object_rx, Float:object_ry, Float:object_rz,
    Float:offset_x, Float:offset_y, Float:offset_z,
    &Float:x, &Float:y, &Float:z)
{
    new
        Float:cos_x = floatcos(object_rx, degrees),
        Float:cos_y = floatcos(object_ry, degrees),
        Float:cos_z = floatcos(object_rz, degrees),
        Float:sin_x = floatsin(object_rx, degrees),
        Float:sin_y = floatsin(object_ry, degrees),
        Float:sin_z = floatsin(object_rz, degrees);

    x = object_px +
    offset_x * cos_y * cos_z - offset_x * sin_x * sin_y * sin_z -
    offset_y * cos_x * sin_z + offset_z * sin_y * cos_z +
    offset_z * sin_x * cos_y * sin_z;
    y = object_py +
    offset_x * cos_y * sin_z + offset_x * sin_x * sin_y * cos_z +
    offset_y * cos_x * cos_z + offset_z * sin_y * sin_z -
    offset_z * sin_x * cos_y * cos_z;
    z = object_pz -
    offset_x * cos_x * sin_y -
    offset_y * sin_x +
    offset_z * cos_x * cos_y;
//I think the issue is in one of the 3 lines above
}

//The function I am trying to get working
stock GetVehicleInfo(vehicleid, infotype, &Float:X, &Float:Y, &Float:Z)
{
    if(!IsValidVehicle(vehicleid))
        return false;

    new Float:mX, Float:mY, Float:mZ, Float:vX, Float:vY, Float:vZ, Float:rX, Float:rY, Float:rZ;

    GetVehicleModelInfo(GetVehicleModel(vehicleid), infotype, mX, mY, mZ);//Get the model's offset to the infotype
    GetVehiclePos(vehicleid, vX, vY, vZ);
    GetVR(vehicleid, rX, rY, rZ);//Get vehicle's rotation

    GetAttachedObjectPos(vX, vY, vZ, rX, rY, rZ,mX, mY, mZ,X, Y, Z);

    return true;

}

public OnPlayerCommandText(playerid, cmdtext[])
{
//Test command
    if (strcmp("/cap", cmdtext, true, 10) == 0)
    {
        if(IsPlayerInAnyVehicle(playerid))
        {
            if(td)
                Delete3DTextLabel(tdt);
            new Float:x, Float:y, Float:z;
            GetVehicleInfo(GetPlayerVehicleID(playerid), VEHICLE_MODEL_INFO_PETROLCAP, x,y,z);
            tdt=Create3DTextLabel("[x]", 0xFFFFFFFF, x,y,z, 50.0, 0, 0);//Create 3d label
            td=1;
            return 1;
        }
    }
    return 0;
}

public OnPlayerCommandText(playerid, cmdtext[])
{
     if(strcmp(cmdtext, "/getvehiclepos", true) == 0)
     {
          new currentveh;
          currentveh = GetPlayerVehicleID(playerid);

          new Float:vehx, Float:vehy, Float:vehz;
          GetVehiclePos(currentveh, vehx, vehy, vehz);

          new vehpostext[96];
          format(vehpostext, sizeof(vehpostext), "The current vehicle positions are: %f, %f, %f pos", vehx, vehy, vehz);

          SendClientMessage(playerid, 0xFFFFFFFF, vehpostext);
          return 1;
     }

     return 0;
}

public OnPlayerCommandText(playerid, cmdtext[])
{
//Test command
    if (strcmp("/cap", cmdtext, true, 10) == 0)
    {
        if(IsPlayerInAnyVehicle(playerid))
        {
            if(td)
            {
                Delete3DTextLabel(tdt);
            }
            new Float:x, Float:y, Float:z, angle;
            GetVehicleZAngle(GetPlayerVehicleID(playerid), angle);
            GetVehicleInfo(GetPlayerVehicleID(playerid), VEHICLE_MODEL_INFO_PETROLCAP, x,y,z, angle);
            tdt = Create3DTextLabel("[x]", 0xFFFFFFFF, x, y, z, 50.0, 0, 0);//Create 3d label
            td = 1;
            return 1;
        }
    }
    if (strcmp("/capattach", cmdtext, true, 10) == 0)
    {
        if(IsPlayerInAnyVehicle(playerid))
        {
            if(td)
            {
                Delete3DTextLabel(tdt);
            }
            new Float:mX, Float:mY, Float:mZ, Floatx, Float:y, Float:z
                vehicleid = GetPlayerVehicleID(playerid);
            GetVehicleModelInfo(GetVehicleModel(vehicleid), VEHICLE_MODEL_INFO_PETROLCAP, mX, mY, mZ); //Get the model's offset to the infotype
            GetVehiclePos(vehicleid, x, y, z);
            tdt = Create3DTextLabel("[x]", 0xFFFFFFFF, x, y, z, 50.0, 0, 0);//Create 3d label
            td = 1;
            Attach3DTextLabelToVehicle(tdt, vehicleid, mX, mY, mZ);
            return 1;
        }
    }
    return 0;
}

stock GetVehicleInfo(vehicleid, infotype, &Float:X, &Float:Y, &Float:Z, &Float:Angle)
{
    if(!IsValidVehicle(vehicleid)) return false;

    new Float:mX, Float:mY, Float:mZ, Float:vX, Float:vY, Float:vZ;

    GetVehicleModelInfo(GetVehicleModel(vehicleid), infotype, mX, mY, mZ);//Get the model's offset to the infotype
    GetVehiclePos(vehicleid, vX, vY, vZ);
    
    z += mZ;

    x += (mY * floatsin(-angle, degrees));
    y += (mY * floatcos(-angle, degrees));

    angle += 270.0;
    x += (mX * floatsin(-angle, degrees));
    y += (mX * floatcos(-angle, degrees));
    angle -= 270.0;
    return true;

}