Sanya-ips/ipstool/ipssps2/Ips.Sps.Scheduling/SigLocs/LocSigGroupWorker.cs

using DevExpress.Utils.Extensions;
using DevExpress.Xpo;
using Ips.Library.Basic;
using Ips.Library.Entity;
using Ips.LocAlgorithm;
using Ips.Sps.Refs;
using Ips.Sps.Scheduling.Entities;
using Ips.Sps.Scheduling.SigProces;
using Ips.Sps.TskResults.Peses;
using Ips.Sps.TskResults.Poses;
using System;
using System.Collections.Generic;
using System.DirectoryServices.ActiveDirectory;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Ips.Sps.Scheduling.SigLocs
{
    public class LocSigGroupWorker : LocWorkerBase
    {
        public LocSigGroupWorker(RunTsk context) : base(context)
        {

        }

        public override void Execute(ProcSigGroup procSigGroup, CancellationToken token = default)
        {
            var tarSigList = procSigGroup.ProcSigList.Where(m => m.Sig.SigCategory != SignalCategory.RefSig && m.PesCorList.Any()).ToList();
            var refSigList = procSigGroup.ProcSigList.Where(m => m.Sig.SigCategory == SignalCategory.RefSig && m.PesCorList.Any()).ToList();
            var refPesList = refSigList.SelectMany(m => m.PesCorList).SelectMany(m => m.PesList).Where(m => m.IsValid).OrderBy(m => m.SigTime).ToList();
            foreach (var tarSigProcResult in tarSigList)
            {
                var sig = tarSigProcResult.Sig;
                if (sig == null) continue;
                IpsLogger.Info($"信号频点：{sig.SigFreq.E6m()}开始定位，信号时间：{procSigGroup.GroupTime:yyyy-MM-dd HH:mm:ss},任务：【{CurrentTsk.TskName}】");
                var mainSatId = sig.MainSatId;
                List<Pos> posList = new List<Pos>();
                try
                {
                    #region 三星定位
                    if (sig.AdjaSatIds.Count >= 2)
                    {
                        for (int i = 0; i < sig.AdjaSatIds.Count - 1; i++)
                        {
                            var adja1SatId = sig.AdjaSatIds[i];
                            var corItem1 = tarSigProcResult.PesCorList.Find(m => m.MainSatId == mainSatId && m.AdjaSatId == adja1SatId);
                            if (corItem1 == null) continue;
                            for (int j = i + 1; j < sig.AdjaSatIds.Count; j++)
                            {
                                var adja2SatId = sig.AdjaSatIds[j];
                                var corItem2 = tarSigProcResult.PesCorList.Find(m => m.MainSatId == mainSatId && m.AdjaSatId == adja2SatId);
                                if (corItem2 == null) continue;
                                var sigPosList = LocDt2(sig, corItem1, corItem2, refPesList, token);
                                if (sigPosList.IsNotNullOrEmpty())
                                {
                                    posList.AddRange(sigPosList);
                                }
                            }
                        }
                    }
                    #endregion

                    #region 双星定位
                    if (CurrentTsk.DfLoc && sig.AdjaSatIds.Count >= 1)
                    {
                        for (int i = 0; i < sig.AdjaSatIds.Count; i++)
                        {
                            var adjaSatId = sig.AdjaSatIds[i];
                            var corItem = tarSigProcResult.PesCorList.Find(m => m.MainSatId == mainSatId && m.AdjaSatId == adjaSatId);
                            if (corItem == null) continue;
                            var sigPosList = LocDtf(sig, corItem, refPesList, token);
                            if (sigPosList.IsNotNullOrEmpty())
                            {
                                posList.AddRange(sigPosList);
                            }
                        }
                    }
                    #endregion

                    PosManager.Default.SendPosListChangedMessage(CurrentTsk.TskId, tarSigProcResult.Sig.SigId, posList, DataChangeType.Insert);
                    IpsLogger.Info($"信号频点：{sig.SigFreq.E6m()}定位完成，信号时间：{procSigGroup.GroupTime:yyyy-MM-dd HH:mm:ss}，任务：【{CurrentTsk.TskName}】");
                }
                catch (Exception ex)
                {
                    IpsLogger.Error($"任务：【{CurrentTsk.TskName}】信号频点：{sig.SigFreq.E6m()}定位异常",ex);
                }
            }
        }

        protected virtual List<Pos> LocDt2(RunSig sig, ProcSigCorItem cor1, ProcSigCorItem cor2, List<Pes> refPesList, CancellationToken token)
        {
            List<Pos> posList = new List<Pos>();
            Session session = new Session();
            foreach (var pes1 in cor1.PesList)
            {
                token.ThrowIfCancellationRequested();
                var startPos = pes1.StartPos;
                var pes2 = cor2.PesList.Find(m => m.StartPos == startPos);
                if (pes2 == null) continue;

                List<Pes> usePesList = new List<Pes>();
                usePesList.AddRange(new[] { pes1, pes2 });

                ExeResult<PosResult> posResult = null;
                Pes refPes1 = null, refPes2 = null;
                List<Pes> refPesList1 = new List<Pes>(), refPesList2 = new List<Pes>();
                if (pes1.IsValid && pes2.IsValid)
                {
                    bool findDbRef = true;// CurrentTsk.SigType != SignalType.NORMAL && !CurrentTsk.IsHistory;
                    //三星单参定位
                    refPes1 = RefManager.Default.GetRefPes(pes1, refPesList, out refPesList1, findDbRef);
                    if (refPes1 != null)
                        refPes2 = RefManager.Default.GetRefPes(pes2, refPesList, out refPesList2, findDbRef);
                    bool hasRef = refPes1 != null && refPes2 != null;
                    if (hasRef)
                    {
                        usePesList.AddRange(refPesList1);
                        usePesList.AddRange(refPesList2);

                        posResult = LocUtil.X3Ref(pes1.Dt, pes2.Dt, refPes1.Dt, refPes2.Dt,
                            refPes1.GetSigLLA(), refPes2.GetSigLLA(),
                            pes1.GetMainAntLLA(), pes1.GetAdjaAntLLA(),
                            pes2.GetMainAntLLA(), pes2.GetAdjaAntLLA(),
                            pes1.GetMainEphXYZ(), pes1.GetAdjaEphXYZ(),
                            pes2.GetMainEphXYZ(), pes2.GetAdjaEphXYZ(),
                            token: token).Result;
                    }
                    else
                    {
                        //三星无参定位
                        posResult = LocUtil.X3(pes1.Dt, pes2.Dt,
                            pes1.GetMainAntLLA(), pes1.GetAdjaAntLLA(),
                            pes2.GetMainAntLLA(), pes2.GetAdjaAntLLA(),
                            pes1.GetMainEphXYZ(), pes1.GetAdjaEphXYZ(),
                            pes2.GetMainEphXYZ(), pes2.GetAdjaEphXYZ(),
                            token: token
                             ).Result;
                    }
                }
                if (posResult != null && posResult.ExitCode > 0)
                {
                    IpsLogger.Error($"定位异常，错误消息：{posResult.ExitMsg},定位参数：{posResult.Arguments}");
                }
                var pos = LocSigUtil.CreatePos(posResult?.Result, CurrentTsk.TskId, sig, pes1.SigTime, session);
                var refSigLla = pos.HasRef ? refPes1.GetSigLLA() : null;

                if (SpsConst.EnableLocGdopErr)
                    pos.GdopErr = LocSigUtil.GetGdopErr(pos, pes1, pes2, refSigLla);
                pos.Save();
                LocSigUtil.SavePosRel(pos, pes1, refPes1, refPesList1, session);
                LocSigUtil.SavePosRel(pos, pes2, refPes2, refPesList2, session);
                posList.Add(pos);
            }
            return posList;
        }

        protected virtual List<Pos> LocDtf(RunSig sig, ProcSigCorItem cor, List<Pes> refPesList, CancellationToken token)
        {
            List<Pos> posList = new List<Pos>();
            Session session = new Session();
            foreach (var pes in cor.PesList)
            {
                token.ThrowIfCancellationRequested();

                List<Pes> usePesList = new List<Pes>();
                usePesList.AddRange(new[] { pes });

                ExeResult<PosResult> posResult = null;
                Pes refPes = null;
                List<Pes> refPesList1 = new List<Pes>();
                if (pes.IsValid)
                {
                    bool findDbRef = true;
                    refPes = RefManager.Default.GetRefPes(pes, refPesList, out refPesList1, findDbRef);
                    bool hasRef = refPes != null;
                    if (hasRef)
                    {
                        usePesList.AddRange(refPesList1);

                        if (SpsConst.EnableDtfLoc32)
                        {
                            posResult = Loc32Util.X2Ref(
                                pes.GetMainAntLLA(),
                                pes.GetAdjaAntLLA(),
                                refPes.GetSigLLA(),
                                pes.SigFreq + SpsConst.DfoLocTurn,
                                refPes.SigFreq + SpsConst.DfoLocTurn,
                                SpsConst.DfoLocTurn,
                                SpsConst.DfoLocTurn,
                                pes.Dt,
                                refPes.Dt,
                                pes.Df,
                                refPes.Df,
                                pes.GetMainEphXYZ(),
                                pes.GetAdjaEphXYZ(),
                                token: token).Result;
                        }
                        else
                        {
                            posResult = LocUtil.X2Ref(
                                pes.GetMainAntLLA(),
                                pes.GetAdjaAntLLA(),
                                refPes.GetSigLLA(),
                                pes.SigFreq + SpsConst.DfoLocTurn,
                                refPes.SigFreq + SpsConst.DfoLocTurn,
                                SpsConst.DfoLocTurn,
                                SpsConst.DfoLocTurn,
                                pes.Dt,
                                refPes.Dt,
                                pes.Df,
                                refPes.Df,
                                pes.GetMainEphXYZ(),
                                pes.GetAdjaEphXYZ(),
                                token: token).Result;
                        }

                    }
                    else
                    {
                        //posResult = LocUtil.X3(pes.Dt, pes2.Dt,
                        //    pes.GetMainAntLLA(), pes.GetAdjaAntLLA(),
                        //    pes2.GetMainAntLLA(), pes2.GetAdjaAntLLA(),
                        //    pes.GetMainEphXYZ(), pes.GetAdjaEphXYZ(),
                        //    pes2.GetMainEphXYZ(), pes2.GetAdjaEphXYZ(),
                        //    token: token
                        //     ).Result;
                    }
                }
                if (posResult != null && posResult.ExitCode > 0)
                {
                    IpsLogger.Error($"定位异常，错误消息：{posResult.ExitMsg},定位参数：{posResult.Arguments}");
                }
                var pos = LocSigUtil.CreatePos(posResult?.Result, CurrentTsk.TskId, sig, pes.SigTime, session);
                var refSigLla = pos.HasRef ? refPes.GetSigLLA() : null;

                pos.Save();
                LocSigUtil.SavePosRel(pos, pes, refPes, refPesList1, session);
                posList.Add(pos);
            }
            return posList;
        }
    }
}