XFEEDToolBox

using System.Runtime.InteropServices;

namespace XFEEDToolBox.Plugin;

internal static unsafe partial class Navigation
{
    internal struct AffinePose
    {
        public Vector right,up,forward,position;
        public AffinePose(Vector right,Vector up,Vector forward,Vector position){this.right=right;this.up=up;this.forward=forward;this.position=position;}
    }
    static volatile uint referenceFailureStage;
    static ulong referenceFailureBodyValue,referenceRadiusGetterValue;
    static ulong referenceFailureBody {get=>Volatile.Read(ref referenceFailureBodyValue);set=>Volatile.Write(ref referenceFailureBodyValue,value);}
    static ulong referenceRadiusGetter {get=>Volatile.Read(ref referenceRadiusGetterValue);set=>Volatile.Write(ref referenceRadiusGetterValue,value);}
    static Vector Add(Vector a,Vector b)=>new(a.x+b.x,a.y+b.y,a.z+b.z);
    static Vector Subtract(Vector a,Vector b)=>new(a.x-b.x,a.y-b.y,a.z-b.z);
    static Vector Scale(Vector a,double scale)=>new(a.x*scale,a.y*scale,a.z*scale);
    static Vector Rotate(Vector value,AffinePose matrix)=>Add(Add(Scale(matrix.right,value.x),Scale(matrix.up,value.y)),Scale(matrix.forward,value.z));
    static AffinePose ReadAffine(nuint at)=>new(Read<Vector>(at),Read<Vector>(at+0x20),Read<Vector>(at+0x40),Read<Vector>(at+0x60));
    static AffinePose Compose(AffinePose a,AffinePose b)=>new(Rotate(a.right,b),Rotate(a.up,b),Rotate(a.forward,b),Add(Rotate(a.position,b),b.position));
    static uint BodyHash(ulong key)
    {
        unchecked {var value=key*0x40000+~key;value=((value>>31)^value)*21;value=((value>>11)^value)*65;return (uint)(value>>22)^(uint)value;}
    }
    static nuint BodyMapEntry(nuint table,ulong capacity,ulong key,uint payload,bool systemMap)
    {
        if(table==0||capacity==0||capacity>65536)return 0;
        var sentinel=table+(nuint)(8*(BodyHash(key)%capacity));var node=Pointer(sentinel);
        for(uint count=0;node!=0&&node!=sentinel&&count<4096;count++,node=Pointer(node))
            if(Read<ulong>(node+8)==key&&(!systemMap||Read<byte>(node+16)==0))return Pointer(node+payload);
        return 0;
    }
    static nuint LoadedSystem(nuint service,ulong world)
    {
        var owner=service-0x58;
        var root=BodyMapEntry(Pointer(owner+0x278),Read<ulong>(owner+0x280),world&0x007FFFFFFFFFFFFFUL,24,true);
        return root!=0&&Read<uint>(root+4)==5&&Read<uint>(root+0x90)!=0?root:0;
    }
    static bool BodyPose(nuint body,nuint universal,nuint resource,AffinePose* output)
    {
        if(Pointer(body+0x378)!=resource)return false;
        var index=Read<uint>(body+0x384);if(index>=4096)return false;
        var transforms=Pointer(resource+0x388);if(transforms==0)return false;
        var result=ReadAffine(universal+0x10);
        if(Read<byte>(universal+0x90)!=0)result=Compose(result,ReadAffine(transforms+index*0x180+0x80));
        result=Compose(result,ReadAffine(transforms+index*0x180+0x100));
        if(Pointer(resource+0x388)!=transforms||!Rotation(result.right,result.up,result.forward)||!Finite(result.position))return false;
        *output=result;return true;
    }
    static double BodyExclusion(nuint body,nuint settings,uint kind,uint starType)
    {
        var getter=Pointer(Pointer(body)+0x10);
        if(Read<uint>(getter)==0x48018B48&&Read<ushort>(getter+4)==0x60FF)getter=Pointer(Pointer(body)+Read<byte>(getter+6));
        referenceRadiusGetter=InImage(getter)?getter-image:0;
        double radius;
        // Read-only main-ship evidence: physical station body kind 6,
        // vtable 553F960, virtual +10 -> 3CF2660 converts the first double
        // at +410 to float. Catalogue kinds (e.g. 6 and 10) are unrelated.
        if(kind==6)
        {
            if(Pointer(body)!=image+0x553F960||getter!=image+0x3CF2660)return double.NaN;
            radius=(float)Read<double>(body+0x410);
        }
        else if(getter==image+0x271B230)radius=Read<float>(body+0x428);
        else if(getter==image+0x3CF26B0)radius=(float)Read<double>(body+0x710);
        else if(getter==image+0x3CF2650)radius=(float)Read<double>(body+0x490);
        else return double.NaN;
        if(!double.IsFinite(radius)||radius<=0||radius>1e14)return double.NaN;
        if(kind==1){nuint offset=starType==0x29?0x2Cu:starType>=0x19&&starType<=0x28?0x34u:0x3Cu;radius=(float)(radius*Read<float>(settings+offset)+Read<float>(settings+offset+4));}
        else if(kind==2)radius=(float)(radius*Read<float>(settings+0x44)+Read<float>(settings+0x48));
        return double.IsFinite(radius)&&radius>0&&radius<=1e14?radius:double.NaN;
    }
    static bool SampleReferenceGeometry(Snapshot frame,GeometryObservation* output)
    {
        try
        {
            referenceFailureStage=1;referenceFailureBody=0;referenceRadiusGetter=0;
            var service=Pointer(frame.fsd+0x6A0);var world=Read<ulong>(frame.transform+0xE8);
            if(world==0||world!=frame.referenceWorld||Pointer(Pointer(service)+0x60)!=image+BodyLookup||Pointer(Pointer(frame.transform)+0x60)!=image+0x2D9C200)return false;
            referenceFailureStage=2;var resource=LoadedSystem(service,world);if(resource==0)return false;
            var table=Pointer(resource+0x140);var capacity=Read<ulong>(resource+0x148);
            if(table==0||capacity==0||capacity>65536)return false;
            referenceFailureStage=3;var reference=BodyMapEntry(table,capacity,world,16,false);
            if(reference==0||Read<ulong>(reference+0x18)!=world)return false;
            referenceFailureStage=4;AffinePose pose=default;if(!BodyPose(reference,frame.transform+0xE8,resource,&pose))return false;
            var settings=Pointer(service-8);if(settings==0)return false;
            referenceFailureStage=5;var multiplier=Read<float>(Pointer(frame.fsd+0x50)+0x44);
            if(!float.IsFinite(multiplier)||multiplier<1||multiplier>10)return false;
            GeometryObservation next=new(){tick=GetTickCount64(),sourceUtcMilliseconds=UtcMilliseconds(),entity=frame.entity,worldAddress=world,
                referenceBodyKind=Read<uint>(reference+0x20),unifiedReference=true,position=pose.position,right=pose.right,up=pose.up,forward=pose.forward};
            var nearest=double.PositiveInfinity;uint visited=0;
            var universal=stackalloc byte[160];
            for(ulong bucket=0;bucket<capacity;bucket++)
            {
                var sentinel=table+(nuint)(bucket*8);var node=Pointer(sentinel);uint chain=0;
                for(;node!=0&&node!=sentinel;node=Pointer(node))
                {
                    if(++chain>4096||++visited>4096)return false;
                    var body=Pointer(node+16);if(body==0)continue;
                    var id=Read<ulong>(body+0x18);var kind=Read<uint>(body+0x20);
                    if(id!=Read<ulong>(node+8)||(id&0x007FFFFFFFFFFFFFUL)!=(world&0x007FFFFFFFFFFFFFUL))return false;
                    if(kind!=1&&kind!=2&&kind!=6&&kind!=7)continue;
                    double stationRadius=double.NaN;
                    if(kind==6)
                    {
                        stationRadius=BodyExclusion(body,settings,kind,0);
                        // This is newly optional coverage. An unknown station
                        // class or unready radius cannot erase the pre-existing
                        // stellar geometry, nor supply any departure authority.
                        // Identity/map corruption above still rejects the frame.
                        if(!double.IsFinite(stationRadius)){next.unverifiedStationEnvelopes++;continue;}
                    }
                    if(next.bodyCount==256)return false;
                    new Span<byte>(universal,160).Clear();
                    *(ulong*)universal=id;*(double*)(universal+0x10)=1;*(double*)(universal+0x38)=1;*(double*)(universal+0x60)=1;
                    referenceFailureBody=id;referenceFailureStage=6;
                    AffinePose centre=default;if(!BodyPose(body,(nuint)universal,resource,&centre))return false;
                    var starType=kind==1?Read<uint>(body+0x448):0;
                    referenceFailureStage=7;var radius=kind==6?stationRadius:BodyExclusion(body,settings,kind,starType);if(!double.IsFinite(radius))return false;
                    next.bodies[(int)next.bodyCount++]=new(){address=id,kind=kind,starType=starType,exclusionRadius=radius,escapeRadius=radius*multiplier,centre=centre.position};
                    var delta=Subtract(pose.position,centre.position);var square=Dot(delta,delta);
                    if(kind==1&&square<nearest){nearest=square;next.bodyAddress=id;next.bodyKind=kind;next.starType=starType;next.exclusionRadius=radius;next.escapeRadius=radius*multiplier;}
                }
                if(node!=sentinel)return false;
            }
            referenceFailureStage=8;if(next.bodyAddress==0||Read<ulong>(frame.transform+0xE8)!=world||LoadedSystem(service,world)!=resource)return false;
            Vector origin=default;for(var i=0;i<next.bodyCount;i++)if(next.bodies[i].address==next.bodyAddress)origin=next.bodies[i].centre;
            // Keep the actual affine-frame translation before reducing the
            // exported stellar geometry to the primary-star origin. A physics
            // input pose or a later local.read ship position is not this origin.
            next.referenceOrigin=origin;next.referenceOriginVerified=Finite(origin)&&next.sourceUtcMilliseconds!=0;
            next.position=Subtract(next.position,origin);
            for(var i=0;i<next.bodyCount;i++)next.bodies[i].centre=Subtract(next.bodies[i].centre,origin);
            SampleScoopGeometry(frame,next,&next.scoop);
            *output=next;referenceFailureStage=0;referenceFailureBody=0;return true;
        }
        catch{return false;}
    }
    static void PollReferenceGeometry(Snapshot frame)
    {
        GeometryObservation previous;lock(geometryLock)previous=geometry;
        if(previous.unifiedReference&&previous.entity==frame.entity&&previous.worldAddress==frame.referenceWorld&&GetTickCount64()-previous.tick<40)return;
        GeometryObservation next=default;if(!SampleReferenceGeometry(frame,&next))return;
        lock(geometryLock)geometry=next;
    }
}