using System;
using System.Linq;
using System.Reflection;
using VRageMath;
using Xunit;
using P=AutoMiningScript.Program;

namespace AutoMiningScript.Tests
{
    public class PortYieldSafetyTests
    {
        static void ObserveFrame(FleetStateTests.Rig rig)
        {
            rig.Program.Now+=.6;
            foreach(var miner in rig.Miners)rig.Receive("TELEMETRY",miner);
            rig.Sent.Clear();rig.Tick(1);
        }

        [Fact]
        public void AnExistingYieldStopsForAThirdShipAndResumesItsOriginalPointWhenClear()
        {
            var rig=new FleetStateTests.Rig(3,false);
            typeof(P.FleetController).GetField("trafficRandom",BindingFlags.Instance|BindingFlags.NonPublic).SetValue(rig.Fleet,new Random(0));
            foreach(var miner in rig.Miners){miner.State=P.FlightState.Holding;miner.Radius=4;miner.ThrustMargin=100;miner.HasRoute=true;}
            rig.Miners[0].Position=new Vector3D(-5,0,-50);rig.Miners[0].Velocity=Vector3D.Right*2;rig.Miners[0].RouteTarget=new Vector3D(30,0,-50);
            rig.Miners[1].Position=new Vector3D(5,0,-50);rig.Miners[1].Velocity=Vector3D.Left*2;rig.Miners[1].RouteTarget=new Vector3D(-30,0,-50);
            var third=rig.Miners[2];third.Position=new Vector3D(1000,1000,-50);third.RouteTarget=third.Position;
            rig.RegisterAll();rig.Tick(1);

            // Obtain the actual randomly selected yielding ship and its original
            // point from traffic packets, rather than installing a private plan.
            var initial=Assert.Single(rig.Sent,p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid"));
            var yielding=rig.Miners.Single(t=>t.Id==initial.Target);
            var priority=rig.Miners.Single(t=>t.Id==P.Data.Text(initial.Body,"traffic","Peer"));
            var point=P.Data.ReadVector(initial.Body,"traffic","Point",Vector3D.Zero);
            var lateral=Vector3D.Normalize(point-yielding.Position);
            Assert.InRange(Vector3D.Distance(point,yielding.Position),1.9,2.01);

            // A subsequent observed frame reports that the yielding ship has
            // started its lateral move and another ship is closing along that leg.
            // This tests one scheduling decision; no movement is integrated here.
            yielding.Velocity=lateral*3;yielding.RouteTarget=point;
            priority.Velocity=Vector3D.Zero;priority.HasRoute=false;
            third.Position=yielding.Position+lateral*11;third.Velocity=-lateral*2;third.RouteTarget=yielding.Position;
            ObserveFrame(rig);

            var blocked=rig.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==yielding.Id);
            Assert.True(P.Data.Flag(blocked.Body,"traffic","Wait"));
            Assert.False(P.Data.Flag(blocked.Body,"traffic","Avoid"));
            Assert.Equal(third.Id,P.Data.Text(blocked.Body,"traffic","Peer"));
            Assert.Equal(point,P.Data.ReadVector(blocked.Body,"traffic","Point",Vector3D.Zero));

            third.Position=new Vector3D(1000,1000,-50);third.Velocity=Vector3D.Zero;third.HasRoute=false;
            ObserveFrame(rig);

            var resumed=rig.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==yielding.Id);
            Assert.False(P.Data.Flag(resumed.Body,"traffic","Wait"));
            Assert.True(P.Data.Flag(resumed.Body,"traffic","Avoid"));
            Assert.Equal(priority.Id,P.Data.Text(resumed.Body,"traffic","Peer"));
            Assert.Equal(point,P.Data.ReadVector(resumed.Body,"traffic","Point",Vector3D.Zero));
        }

        static FleetStateTests.Rig Encounter(Vector3D first,Vector3D second,Vector3D firstVelocity,Vector3D secondVelocity,double thrustMargin=100,double response=.625)
        {
            var r=new FleetStateTests.Rig(2,false);
            for(int n=0;n<2;n++)
            {
                var t=r.Miners[n];t.State=P.FlightState.Transit;t.Radius=3;t.ThrustMargin=thrustMargin;t.BrakeResponse=response;t.HasRoute=true;
                t.Position=n==0?first:second;t.Velocity=n==0?firstVelocity:secondVelocity;t.RouteTarget=t.Position+t.Velocity*30;
            }
            r.RegisterAll();r.Tick(1);return r;
        }

        [Fact]
        public void SlowDistantConvergenceAndNearbySafeMissesDoNotTriggerYielding()
        {
            var slow=Encounter(new Vector3D(-7.5,0,-50),new Vector3D(7.5,0,-50),Vector3D.Right*.2,Vector3D.Left*.2);
            var miss=Encounter(new Vector3D(-5,0,-50),new Vector3D(5,0,-42),Vector3D.Right*2,Vector3D.Left*2);
            foreach(var r in new[]{slow,miss})
                Assert.All(r.Sent.Where(p=>p.Kind=="TRAFFIC"),p=>{Assert.False(P.Data.Flag(p.Body,"traffic","Wait"));Assert.False(P.Data.Flag(p.Body,"traffic","Avoid"));});
            Assert.Equal(2,slow.Sent.Count(p=>p.Kind=="TRAFFIC"));Assert.Equal(2,miss.Sent.Count(p=>p.Kind=="TRAFFIC"));
        }

        [Theory]
        [InlineData(0,false)]
        [InlineData(.1,true)]
        public void UnknownBrakingDoesNotInventAnEarlyEncounterButKnownLowBrakingRemainsProtected(double thrustMargin,bool mustYield)
        {
            var r=Encounter(new Vector3D(-25,0,-50),new Vector3D(25,0,-50),Vector3D.Right*3,Vector3D.Left*3,thrustMargin);
            var responses=r.Sent.Where(p=>p.Kind=="TRAFFIC").ToList();
            Assert.Equal(2,responses.Count);
            if(mustYield)Assert.Single(responses,p=>P.Data.Flag(p.Body,"traffic","Avoid"));
            else Assert.All(responses,p=>{Assert.False(P.Data.Flag(p.Body,"traffic","Wait"));Assert.False(P.Data.Flag(p.Body,"traffic","Avoid"));});
        }

        [Fact]
        public void AnActiveYieldEndsWhenThePeerTurnsAwayEvenThoughItStillHasARoute()
        {
            var r=Encounter(new Vector3D(-4.5,0,-50),new Vector3D(4.5,0,-50),Vector3D.Right*2,Vector3D.Left*2);
            var initial=Assert.Single(r.Sent,p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid"));
            var yielding=r.Miners.Single(t=>t.Id==initial.Target);
            var peer=r.Miners.Single(t=>t.Id==P.Data.Text(initial.Body,"traffic","Peer"));

            // The peer now travels away at the yielding ship's speed. Their
            // measured relative motion is safe, while the old encounter velocity
            // would still describe a head-on collision. HasRoute stays true.
            peer.Velocity=Vector3D.Normalize(peer.Position-yielding.Position)*2;
            peer.RouteTarget=peer.Position+peer.Velocity*30;peer.HasRoute=true;
            ObserveFrame(r);

            var clear=r.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==yielding.Id);
            Assert.True(peer.HasRoute);
            Assert.False(P.Data.Flag(clear.Body,"traffic","Wait"));
            Assert.False(P.Data.Flag(clear.Body,"traffic","Avoid"));
        }

        [Fact]
        public void BothEndsKeepTheSameEncounterUntilTheOriginalCoursesAreClear()
        {
            var r=Encounter(new Vector3D(-4.5,0,-50),new Vector3D(4.5,0,-50),Vector3D.Right*2,Vector3D.Left*2);
            var initial=Assert.Single(r.Sent,p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid"));
            var loser=r.Miners.Single(t=>t.Id==initial.Target);var peer=r.Miners.Single(t=>t.Id==P.Data.Text(initial.Body,"traffic","Peer"));
            var side=Vector3D.Normalize(P.Data.ReadVector(initial.Body,"traffic","Point")-loser.Position);
            loser.Velocity=side*.2;peer.Velocity=Vector3D.Zero;ObserveFrame(r);
            var waiting=r.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==peer.Id);
            Assert.True(P.Data.Flag(waiting.Body,"traffic","Wait"));Assert.False(P.Data.Flag(waiting.Body,"traffic","Avoid"));
            Assert.True(P.Data.Flag(r.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==loser.Id).Body,"traffic","Avoid"));
            loser.Position+=side*7;loser.Velocity=Vector3D.Zero;ObserveFrame(r);
            Assert.All(r.Sent.Where(p=>p.Kind=="TRAFFIC"),p=>{Assert.False(P.Data.Flag(p.Body,"traffic","Wait"));Assert.False(P.Data.Flag(p.Body,"traffic","Avoid"));});
        }

        [Fact]
        public void NearPortSideStepsStayOutsideTheConnectorPlaneAndAreOnlyTwoMetres()
        {
            var r=Encounter(new Vector3D(-4.5,0,-4),new Vector3D(4.5,0,-5),Vector3D.Right*2,Vector3D.Left*2);
            var packet=Assert.Single(r.Sent,p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid"));
            var miner=r.Miners.Single(t=>t.Id==packet.Target);var step=P.Data.ReadVector(packet.Body,"traffic","Point",Vector3D.Zero)-miner.Position;
            Assert.InRange(step.Length(),.1,2.01);
            Assert.True(Vector3D.Dot(miner.Position+step,Vector3D.Forward)>=miner.Radius-.001);
        }

        [Fact]
        public void APortPlaneDoesNotReverseASafeShortStepTowardsTheOtherMiner()
        {
            var r=new FleetStateTests.Rig(2,false);r.SetPort(0,new Vector3D(-40,0,0),Vector3D.Right);r.Tick(1);
            var loser=r.Miners[0];var winner=r.Miners[1];loser.Position=new Vector3D(-13,0,1);winner.Position=new Vector3D(-10,0,-4);winner.Velocity=Vector3D.Forward;
            var side=(Vector3D)typeof(P.FleetController).GetMethod("YieldSide",BindingFlags.Instance|BindingFlags.NonPublic).Invoke(r.Fleet,new object[]{loser,winner});
            Assert.True(side.X<-.9);Assert.True(Vector3D.Dot(side,loser.Position-winner.Position)>=0);
            Assert.True(Vector3D.Dot(loser.Position+side*2-new Vector3D(-40,0,0),Vector3D.Right)>=loser.Radius);
        }

        [Theory]
        [InlineData(.2,false)]
        [InlineData(4,true)]
        public void CommonTranslationDoesNotChangeTheActualEncounterDecision(double closure,bool expected)
        {
            foreach(var drift in new[]{Vector3D.Zero,Vector3D.Right*20,Vector3D.Up*20,new Vector3D(20,20,20)})
            {
                var r=Encounter(new Vector3D(-7.5,0,-50),new Vector3D(7.5,0,-50),drift+Vector3D.Right*closure,drift+Vector3D.Left*closure);
                var packets=r.Sent.Where(p=>p.Kind=="TRAFFIC").ToList();Assert.Equal(2,packets.Count);
                Assert.Equal(expected,packets.Any(p=>P.Data.Flag(p.Body,"traffic","Avoid")));
            }
        }

        [Theory]
        [InlineData(.625,false)]
        [InlineData(5,true)]
        public void SlowFeedbackBrakesEarlyEnoughEvenWithPlentifulHardwareThrust(double response,bool expected)
        {
            var r=Encounter(new Vector3D(-10,0,-50),new Vector3D(10,0,-50),Vector3D.Right*2,Vector3D.Left*2,100,response);
            Assert.Equal(expected,r.Sent.Any(p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid")));
        }

        [Fact]
        public void OldTelemetryDefaultsToTheExistingFeedbackResponse()
        {
            var body=new P.Telemetry().ToIni();body.Delete("telemetry","BrakeResponse");
            Assert.Equal(.625,P.Telemetry.FromIni(body).BrakeResponse);
            foreach(double invalid in new[]{0d,.2,5.1,double.NaN,double.PositiveInfinity})
            {body.Set("telemetry","BrakeResponse",invalid);Assert.Throws<ArgumentException>(()=>P.Telemetry.FromIni(body));}
        }

        [Fact]
        public void YieldEndsAsSoonAsResumingTheEncounterCourseIsSafeWithoutFortyFiveMetresSeparation()
        {
            var r=Encounter(new Vector3D(-4.5,0,-50),new Vector3D(4.5,0,-50),Vector3D.Right*2,Vector3D.Left*2);
            var initial=Assert.Single(r.Sent,p=>p.Kind=="TRAFFIC"&&P.Data.Flag(p.Body,"traffic","Avoid"));
            var yielding=r.Miners.Single(t=>t.Id==initial.Target);var point=P.Data.ReadVector(initial.Body,"traffic","Point",Vector3D.Zero);
            yielding.Position+=Vector3D.Normalize(point-yielding.Position)*7;yielding.Velocity=Vector3D.Zero;
            ObserveFrame(r);var clear=r.Sent.Last(p=>p.Kind=="TRAFFIC"&&p.Target==yielding.Id);
            Assert.False(P.Data.Flag(clear.Body,"traffic","Wait"));Assert.False(P.Data.Flag(clear.Body,"traffic","Avoid"));
            Assert.True(Vector3D.Distance(r.Miners[0].Position,r.Miners[1].Position)<20);
        }
    }
}