Sanya-ips/ipstool/ipssps2/Ips.Sps.Core/Refs/RefManager.cs

using DevExpress.Xpo;
using Ips.Library.Basic;
using Ips.Library.Entity;
using Ips.LocAlgorithm;
using Ips.Sps.Ephs;
using Ips.Sps.TskResults.Peses;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Ips.Sps.Refs
{
    public sealed class RefManager
    {
        public static readonly RefManager Default = new RefManager();

        private RefManager()
        {

        }

        public static int RefFindMin { get; set; } = 30;

        public Pes GetRefPes(Pes tarPes, List<Pes> pesList, out List<Pes> usePesList, bool findDbRef = false)
        {
            usePesList = new List<Pes>();
            Pes pesRef = null;
            pesList = pesList?.Where(m => m.IsValid)?.OrderBy(m => Math.Abs((m.SigTime - tarPes.SigTime).TotalSeconds))?.ToList();
            if (pesList.IsNotNullOrEmpty())
            {
                var maRefPes = pesList.FirstOrDefault(m => (m.MainSatId == tarPes.MainSatId && m.AdjaSatId == tarPes.AdjaSatId) || (m.MainSatId == tarPes.AdjaSatId && m.AdjaSatId == tarPes.MainSatId));
                if (maRefPes != null)
                {
                    pesRef = CreateRefPes(tarPes, maRefPes);
                    usePesList.Add(maRefPes);
                }
                else
                {
                    var mRefPes = pesList.FirstOrDefault(m => m.AdjaSatId == tarPes.MainSatId);
                    if (mRefPes != null)
                    {
                        var aRefPes = pesList.FirstOrDefault(m => m.EmtId == mRefPes.EmtId && m.MainSatId == mRefPes.MainSatId && m.AdjaSatId == tarPes.AdjaSatId);
                        if (aRefPes != null)
                        {
                            usePesList.AddRange(new[] { mRefPes, aRefPes });
                            pesRef = CreateRefPes(tarPes, mRefPes, aRefPes);
                        }
                    }
                }
            }

            if (findDbRef && pesRef == null)
            {
                Session session = new Session();
                var minSigTime = tarPes.SigTime.AddMinutes(-RefFindMin);
                var maxSigTime = tarPes.SigTime.AddMinutes(RefFindMin);
                var maRefPes = session.Query<Pes>()
                    .Where(m => m.Category == SignalCategory.RefSig && m.IsValid)
                    .Where(m => (m.MainSatId == tarPes.MainSatId && m.AdjaSatId == tarPes.AdjaSatId) || (m.MainSatId == tarPes.AdjaSatId && m.AdjaSatId == tarPes.MainSatId))
                    .Where(m => m.SigTime <= tarPes.SigTime && m.SigTime >= minSigTime)
                    .OrderByDescending(m => m.SigTime)
                    .FirstOrDefault();
                if (maRefPes != null)
                {
                    pesRef = CreateRefPes(tarPes, maRefPes);
                    usePesList.Add(maRefPes);
                }
                else
                {
                    var mRefPes = session.Query<Pes>()
                        .Where(m => m.Category == SignalCategory.RefSig && m.IsValid)
                        .Where(m => m.AdjaSatId == tarPes.MainSatId)
                        .Where(m => m.SigTime <= tarPes.SigTime && m.SigTime >= minSigTime)
                        .OrderByDescending(m => m.SigTime)
                        .FirstOrDefault();
                    if (mRefPes != null)
                    {
                        var aRefPes = session.Query<Pes>()
                            .Where(m => m.Category == SignalCategory.RefSig && m.IsValid)
                                .Where(m => m.EmtId == mRefPes.EmtId && m.MainSatId == mRefPes.MainSatId && m.AdjaSatId == tarPes.AdjaSatId)
                                .Where(m => m.SigTime <= tarPes.SigTime && m.SigTime >= minSigTime)
                                .OrderByDescending(m => m.SigTime)
                                .FirstOrDefault();
                        if (aRefPes != null)
                        {
                            pesRef = CreateRefPes(tarPes, mRefPes, aRefPes);
                            usePesList.AddRange(new[] { mRefPes, aRefPes });
                        }
                    }
                }
            }

            return pesRef;
        }

        private Pes CreateRefPes(Pes tarPes, Pes maRefPes)
        {
            bool isMain = maRefPes.MainSatId == tarPes.MainSatId;
            var refPes = new Pes();
            refPes.SigId = maRefPes.SigId;
            refPes.EmtId = maRefPes.EmtId;
            refPes.SigTime = maRefPes.SigTime;
            refPes.Category = SignalCategory.RefSig;
            refPes.SigFreq = maRefPes.SigFreq;
            refPes.SigType = maRefPes.SigType;
            refPes.SigLon = maRefPes.SigLon;
            refPes.SigLat = maRefPes.SigLat;
            refPes.SigAlt = maRefPes.SigAlt;
            refPes.LonMin = maRefPes.LonMin;
            refPes.LonMax = maRefPes.LonMax;
            refPes.LatMin = maRefPes.LatMin;
            refPes.LatMax = maRefPes.LatMax;


            refPes.MainSatId = isMain ? maRefPes.MainSatId : maRefPes.AdjaSatId;
            refPes.MainSatNum = isMain ? maRefPes.MainSatNum : maRefPes.AdjaSatNum;
            refPes.MainAntId = isMain ? maRefPes.MainAntId : maRefPes.AdjaAntId;
            refPes.MainAntLon = isMain ? maRefPes.MainAntLon : maRefPes.AdjaAntLon;
            refPes.MainAntLat = isMain ? maRefPes.MainAntLat : maRefPes.AdjaAntLat;
            refPes.MainAntAlt = isMain ? maRefPes.MainAntAlt : maRefPes.MainAntAlt;
            refPes.MainEphX = isMain ? maRefPes.MainEphX : maRefPes.AdjaEphX;
            refPes.MainEphY = isMain ? maRefPes.MainEphY : maRefPes.AdjaEphY;
            refPes.MainEphZ = isMain ? maRefPes.MainEphZ : maRefPes.AdjaEphZ;
            refPes.MainEphVx = isMain ? maRefPes.MainEphVx : maRefPes.AdjaEphVx;
            refPes.MainEphVy = isMain ? maRefPes.MainEphVy : maRefPes.AdjaEphVy;
            refPes.MainEphVz = isMain ? maRefPes.MainEphVz : maRefPes.AdjaEphVz;

            refPes.AdjaSatId = !isMain ? maRefPes.MainSatId : maRefPes.AdjaSatId;
            refPes.AdjaSatNum = !isMain ? maRefPes.MainSatNum : maRefPes.AdjaSatNum;
            refPes.AdjaAntId = !isMain ? maRefPes.MainAntId : maRefPes.AdjaAntId;
            refPes.AdjaAntLon = !isMain ? maRefPes.MainAntLon : maRefPes.AdjaAntLon;
            refPes.AdjaAntLat = !isMain ? maRefPes.MainAntLat : maRefPes.AdjaAntLat;
            refPes.AdjaAntAlt = !isMain ? maRefPes.MainAntAlt : maRefPes.MainAntAlt;
            refPes.AdjaEphX = !isMain ? maRefPes.MainEphX : maRefPes.AdjaEphX;
            refPes.AdjaEphY = !isMain ? maRefPes.MainEphY : maRefPes.AdjaEphY;
            refPes.AdjaEphZ = !isMain ? maRefPes.MainEphZ : maRefPes.AdjaEphZ;
            refPes.AdjaEphVx = !isMain ? maRefPes.MainEphVx : maRefPes.AdjaEphVx;
            refPes.AdjaEphVy = !isMain ? maRefPes.MainEphVy : maRefPes.AdjaEphVy;
            refPes.AdjaEphVz = !isMain ? maRefPes.MainEphVz : maRefPes.AdjaEphVz;

            refPes.StartPos = maRefPes.StartPos;
            refPes.Length = maRefPes.Length;
            refPes.IsValid = true;
            refPes.Dt = maRefPes.Dt * (isMain ? 1 : -1);
            refPes.Df = maRefPes.Df * (isMain ? 1 : -1);
            refPes.Snr = maRefPes.Snr;

            refPes.Dt = CalcRefDt(tarPes, refPes);
            refPes.Df = refPes.Df;
            //if (SpsConst.EnableDtfLoc)
            //{
            //    refPes.Df = CalcRefDf(tarPes, refPes) + SpsConst.DefaultStdfErr;
            //}
            refPes.SigTime = tarPes.SigTime;
            refPes.MainEphX = tarPes.MainEphX;
            refPes.MainEphY = tarPes.MainEphY;
            refPes.MainEphZ = tarPes.MainEphZ;
            refPes.MainEphVx = tarPes.MainEphVx;
            refPes.MainEphVy = tarPes.MainEphVy;
            refPes.MainEphVz = tarPes.MainEphVz;
            refPes.AdjaEphX = tarPes.AdjaEphX;
            refPes.AdjaEphY = tarPes.AdjaEphY;
            refPes.AdjaEphZ = tarPes.AdjaEphZ;
            refPes.AdjaEphVx = tarPes.AdjaEphVx;
            refPes.AdjaEphVy = tarPes.AdjaEphVy;
            refPes.AdjaEphVz = tarPes.AdjaEphVz;

            return refPes;
        }

        private Pes CreateRefPes(Pes tarPes, Pes mRefPes, Pes aRefPes)
        {
            var pesRef = new Pes();
            pesRef.SigId = mRefPes.SigId;
            pesRef.EmtId = mRefPes.EmtId;
            pesRef.SigTime = tarPes.SigTime;
            pesRef.Category = SignalCategory.RefSig;
            pesRef.SigFreq = mRefPes.SigFreq;
            pesRef.SigType = mRefPes.SigType;
            pesRef.SigLon = mRefPes.SigLon;
            pesRef.SigLat = mRefPes.SigLat;
            pesRef.SigAlt = mRefPes.SigAlt;
            pesRef.LonMin = mRefPes.LonMin;
            pesRef.LonMax = mRefPes.LonMax;
            pesRef.LatMin = mRefPes.LatMin;
            pesRef.LatMax = mRefPes.LatMax;

            pesRef.MainSatId = mRefPes.AdjaSatId;
            pesRef.MainSatNum = mRefPes.AdjaSatNum;
            pesRef.MainAntId = mRefPes.AdjaAntId;
            pesRef.MainAntLon = mRefPes.AdjaAntLon;
            pesRef.MainAntLat = mRefPes.AdjaAntLat;
            pesRef.MainAntAlt = mRefPes.AdjaAntAlt;
            pesRef.MainEphX = tarPes.MainEphX;
            pesRef.MainEphY = tarPes.MainEphY;
            pesRef.MainEphZ = tarPes.MainEphZ;

            pesRef.AdjaSatId = aRefPes.AdjaSatId;
            pesRef.AdjaSatNum = aRefPes.AdjaSatNum;
            pesRef.AdjaAntId = aRefPes.AdjaAntId;
            pesRef.AdjaAntLon = aRefPes.AdjaAntLon;
            pesRef.AdjaAntLat = aRefPes.AdjaAntLat;
            pesRef.AdjaAntAlt = aRefPes.AdjaAntAlt;
            pesRef.AdjaEphX = tarPes.AdjaEphX;
            pesRef.AdjaEphY = tarPes.AdjaEphY;
            pesRef.AdjaEphZ = tarPes.AdjaEphZ;

            pesRef.StartPos = mRefPes.StartPos;
            pesRef.Length = mRefPes.Length;
            pesRef.Df = aRefPes.Df - mRefPes.Df;
            pesRef.Snr = Math.Min(mRefPes.Snr, aRefPes.Snr);
            pesRef.IsValid = true;
            var mRefDt = CalcRefDt(tarPes, mRefPes);
            var aRefDt = CalcRefDt(tarPes, aRefPes);
            pesRef.Dt = aRefDt - mRefDt;
            return pesRef;
        }

        private double CalcRefDt(Pes tarPes, Pes refPes)
        {
            double refDt = refPes.Dt;
            if (Math.Abs((tarPes.SigTime - refPes.SigTime).TotalSeconds) < 2)
                return refDt;
            if (refPes.MainSatId > 0)
            {
                var refTimeDt = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), refPes.GetMainEphXYZ(), refPes.GetAdjaEphXYZ(), refPes.GetMainAntLLA(), refPes.GetAdjaAntLLA());
                var tarTimeDt = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), tarPes.GetMainEphXYZ(), tarPes.GetAdjaEphXYZ(), refPes.GetMainAntLLA(), refPes.GetAdjaAntLLA());
                refDt = refPes.Dt - (refTimeDt - tarTimeDt);
            }
            else
            {
                var refTimeDt = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), refPes.GetAdjaEphXYZ(), refPes.GetAdjaAntLLA());
                bool isMain = tarPes.MainSatId == refPes.AdjaSatId;
                var tarEphX = isMain ? tarPes.MainEphX : tarPes.AdjaEphX;
                var tarEphY = isMain ? tarPes.MainEphY : tarPes.AdjaEphY;
                var tarEphZ = isMain ? tarPes.MainEphZ : tarPes.AdjaEphZ;
                var tarEphXyz = new[] { tarEphX, tarEphY, tarEphZ };
                var tarTimeDt = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), tarEphXyz, refPes.GetAdjaAntLLA());
                refDt = refPes.Dt - (refTimeDt - tarTimeDt);
            }
            return refDt;
        }

        private double CalcRefDf(Pes tarPes, Pes refPes)
        {
            double refDf = refPes.Df;
            bool isSameTime = Math.Abs((tarPes.SigTime - refPes.SigTime).TotalSeconds) < 2;
            if (isSameTime && (!SpsConst.EnableCatchFixError || tarPes.EmtId == 0))
            {
                return refDf;
            }

            var refLla = refPes.GetSigLLA();
            var mainAnt = refPes.GetMainAntLLA();
            var adjaAnt = refPes.GetAdjaAntLLA();
            var mainEph1 = refPes.GetMainEphXYZ();
            var adjaEph1 = refPes.GetAdjaEphXYZ();
            var mainEph2 = tarPes.GetMainEphXYZ();
            var adjaEph2 = tarPes.GetAdjaEphXYZ();

            var refFreqUp = refPes.SigFreq + SpsConst.DfoLocTurn;
            var turn = SpsConst.DfoLocTurn;

            //var mainEph1 = EphManager.Default.GetEph(refPes.MainSatNum, refPes.SigTime).GetEphXyz();
            //var adjaEph1 = EphManager.Default.GetEph(refPes.AdjaSatNum, refPes.SigTime).GetEphXyz();
            //var mainEph2 = EphManager.Default.GetEph(tarPes.MainSatNum, tarPes.SigTime).GetEphXyz();
            //var adjaEph2 = EphManager.Default.GetEph(tarPes.AdjaSatNum, tarPes.SigTime).GetEphXyz();

            if (refPes.MainSatId > 0)
            {
                //var refTimeDf = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), refPes.GetMainEphXYZ(), refPes.GetAdjaEphXYZ(), refPes.GetMainAntLLA(), refPes.GetAdjaAntLLA());
                //var tarTimeDf = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), tarPes.GetMainEphXYZ(), tarPes.GetAdjaEphXYZ(), refPes.GetMainAntLLA(), refPes.GetAdjaAntLLA());
                var refTimeDf = LocUtil.CalcDf(refFreqUp, turn, turn, refLla, mainEph1, adjaEph1, mainAnt, adjaAnt).Result.Result;
                var tarTimeDf = LocUtil.CalcDf(refFreqUp, turn, turn, refLla, mainEph2, adjaEph2, mainAnt, adjaAnt).Result.Result;

                refDf = refPes.Df - (refTimeDf - tarTimeDf);
            }
            else
            {
                //var refTimeDf = LocUtil.CalcDf(refFreqUp, turn, turn, refLla, mainEph1, adjaEph1, mainAnt, adjaAnt).Result.Result;
                //bool isMain = tarPes.MainSatId == refPes.AdjaSatId;
                //var tarEphX = isMain ? tarPes.MainEphX : tarPes.AdjaEphX;
                //var tarEphY = isMain ? tarPes.MainEphY : tarPes.AdjaEphY;
                //var tarEphZ = isMain ? tarPes.MainEphZ : tarPes.AdjaEphZ;
                //var tarEphXyz = new[] { tarEphX, tarEphY, tarEphZ };
                //var tarTimeDf = GeoUtil.CalcDtoCenter(refPes.GetSigLLA(), tarEphXyz, refPes.GetAdjaAntLLA());
                //refDf = refPes.Df - (-tart);
            }
            if (SpsConst.EnableCatchFixError)
            {
                var dfRefDiff = CalcDfCenter(tarPes, refPes);
                if (dfRefDiff.HasValue)
                {
                    var realDiff = tarPes.Df - refDf;
                    var errDfDiff = Math.Abs(dfRefDiff.Value - realDiff);
                    var realDfNew = dfRefDiff.Value + errDfDiff * (isSameTime ? 0.4 : 0.6);
                    refDf = tarPes.Df - realDfNew;
                }
            }

            return refDf;
        }

        public double[] CalcDtCR(DateTime sigTime, long sigFreq, int bandWidth, int mainAntId, int adjaAntId, EphResult mainEph, EphResult adjaEph)
        {
            var pes = GetSigLastPes(sigTime, sigFreq, bandWidth, mainAntId, adjaAntId, mainEph?.SatNum ?? 0, adjaEph.SatNum);
            if (pes == null) return null;
            double[] dtcr = null;
            if (mainAntId > 0 && adjaAntId > 0)
            {
                var dtc = GeoUtil.CalcLeadDt(pes.Dt, pes.GetSigLLA(), pes.GetMainAntLLA(), pes.GetAdjaAntLLA(), pes.GetMainEphXYZ(), pes.GetAdjaEphXYZ(), mainEph.GetEphXyz(), adjaEph.GetEphXyz());
                dtcr = new double[2] { dtc, 200 };
            }
            else if (adjaAntId > 0)
            {
                var dtc = GeoUtil.CalcLeadDt(pes.Dt, pes.GetSigLLA(), pes.GetAdjaAntLLA(), pes.GetAdjaEphXYZ(), adjaEph.GetEphXyz());
                dtcr = new double[2] { dtc, 200 };
            }
            return dtcr;
        }

        private double? CalcDfCenter(Pes tarPes, Pes refPes)
        {
            if (tarPes.EmtId == 0 || refPes == null) return null;

            var msAnt = tarPes.GetMainAntLLA();
            var nsAnt = tarPes.GetAdjaAntLLA();
            var msEph = tarPes.GetMainEphXYZ();
            var nsEph = tarPes.GetAdjaEphXYZ();
            var dt = tarPes.Dt;
            var df = tarPes.Df;
            var dtRef = refPes?.Dt ?? 0;
            var dfRef = refPes?.Df ?? 0;

            var freqUp = tarPes.SigFreq + SpsConst.DfoLocTurn;
            var refFreqUp = (refPes?.SigFreq ?? 0) + SpsConst.DfoLocTurn;
            var turn = SpsConst.DfoLocTurn;
            var refLla = refPes?.GetSigLLA() ?? null;
            var pos = tarPes.GetSigLLA();


            var df0 = Task.Run(
                    () => Loc32Util.CalcDf(freqUp, turn, turn, tarPes.GetSigLLA(), msEph, nsEph, msAnt, nsAnt, refFreqUp, refLla).Result.Result).Result;
            return df0;
        }

        private Pes GetSigLastPes(DateTime sigTime, long sigFreq, int bandWidth, int mainAntId, int adjaAntId, int mainSatNum, int adjaSatNum)
        {
            Session session = new Session();
            var minSigTime = sigTime.AddMinutes(-RefFindMin);
            var pes = session.Query<Pes>()
                .Where(m => m.SigTime <= sigTime && m.SigTime >= minSigTime)
                .Where(m => m.SigFreq == sigFreq && m.BandWidth == bandWidth)
                .Where(m => m.MainAntId == mainAntId && m.AdjaAntId == adjaAntId)
                .Where(m => m.MainSatNum == mainSatNum && m.AdjaSatNum == adjaSatNum)
                .Where(m => m.IsValid && m.Snr > 15)
                .OrderByDescending(m => m.SigTime)
                .FirstOrDefault();
            return pes;
        }
    }
}