XdCxRhDW/XdCxRhDW.Api/AddIns/GDOP误差椭圆/GdopHelper.cs


using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;

namespace XdCxRhDW.Api
{
    public static class GdopHelper
    {

        static readonly DateTime dtZero = new DateTime(1970, 1, 1, 8, 0, 0, 0);

        /// <summary>
        /// 两星一地GDOP 无参时参考位置不赋值
        /// </summary>
        /// <param name="mainLines">主星星历</param>
        /// <param name="adajLines">邻星星历</param>
        /// <param name="captime">信号时间</param>
        /// <param name="cdbPos">超短波位置 3</param>
        /// <param name="refPos">参考站位置 3</param>
        /// <param name="dtousErr">时差误差</param>
        /// <param name="ephLocErr">星历误差</param>
        /// <returns></returns>
        public static (List<MapSatInfo>, List<ErrDistanceMapPoints>) Gdop2Sat1D(string mainLines, string adajLines, DateTime captime, double[] cdbPos, double dtousErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 2;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            var timeSpan = (long)(captime - dtZero).TotalSeconds;
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.Gdop2Sat1DNoRef(mainLines, adajLines, timeSpan, cdbPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }
            else
            {

                GDOPApi.Gdop2Sat1DRef(mainLines, adajLines, timeSpan, cdbPos, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }


            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<MapSatInfo> satInfos = ParseResult(satCount, satllh, mainLines, adajLines);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return (satInfos, errs);
        }

        /// <summary>
        /// 两星一地GDOP 无参时参考位置不赋值
        /// </summary>
        /// <param name="mainEph">主星星历 x y z vx vy vz</param>
        /// <param name="adajEph">邻星星历x y z vx vy vz</param>
        /// <param name="cdbPos">超短波位置 3</param>
        /// <param name="refPos">参考站位置 3</param>
        /// <param name="dtousErr">时差误差</param>
        /// <param name="ephLocErr">星历误差</param>
        /// <param name="refPos"></param>
        /// <returns></returns>
        public static  List<ErrDistanceMapPoints> Gdop2Sat1DByXyz(double[] mainEph, double[] adajEph, double[] cdbPos, double dtousErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 2;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.Gdop2Sat1DNoRefByXyz(mainEph, adajEph, cdbPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }
            else
            {

                GDOPApi.Gdop2Sat1DRefByXyz(mainEph, adajEph, cdbPos, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }


            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return  errs;
        }
        /// <summary>
        /// 一星一地GDOP 
        /// </summary>
        /// <param name="mainLines">主星星历</param>
        /// <param name="captime">信号时间</param>
        /// <param name="cdbPos">超短波位置 3</param>
        /// <param name="cxPos">测向站位置 3</param>
        /// <param name="dtousErr">时差误差</param>
        /// <param name="doaErr">测向误差</param>
        /// <param name="ephLocErr">星历误差</param>
        /// <param name="refPos">参考站位置 3</param>
        /// <returns></returns>
        public static (List<MapSatInfo>, List<ErrDistanceMapPoints>) Gdop1Sat1D(string mainLines, DateTime captime, double[] cdbPos, double[] cxPos, double dtousErr, double doaErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 1;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            var timeSpan = (long)(captime - dtZero).TotalSeconds;
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.GdopXDCXNoRef(mainLines, timeSpan, cdbPos, cxPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
            }
            else
            {
                GDOPApi.GdopXDCXRef(mainLines, timeSpan, cdbPos, cxPos, refPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
            }
            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<MapSatInfo> satInfos = ParseResult(satCount, satllh, mainLines);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return (satInfos, errs);
        }

        public static  List<ErrDistanceMapPoints> Gdop1Sat1DByXyz(double[] mainEph, double[] cdbPos, double[] cxPos, double dtousErr, double doaErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 1;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];


            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.GdopXDCXNoRefByXyz(mainEph, cdbPos, cxPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
            }
            else
            {
                GDOPApi.GdopXDCXRefByXyz(mainEph, cdbPos, cxPos, refPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
            }
            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return  errs;
        }

        public static (List<MapSatInfo>, List<ErrDistanceMapPoints>) Gdop3Sat(string mainLines, string adajLines, string adajLines2,
            DateTime captime, double dtousErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 3;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            var timeSpan = (long)(captime - dtZero).TotalSeconds;
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.Gdop3SatNoRef(mainLines, adajLines, adajLines2, timeSpan, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }
            else
            {
                GDOPApi.Gdop3SatRef(mainLines, adajLines, adajLines2, timeSpan, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }


            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<MapSatInfo> satInfos = ParseResult(satCount, satllh, mainLines, adajLines, adajLines2);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return (satInfos, errs);
        }
        public static List<ErrDistanceMapPoints> Gdop3SatByXyz(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
         double dtousErr, double ephLocErr, double[] refPos = null)
        {
            int satCount = 3;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

          
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.Gdop3SatNoRefByXyz(mainEph, adaj1Eph, adaj2Eph, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }
            else
            {
                GDOPApi.Gdop3SatRefByXyz(mainEph, adaj1Eph, adaj2Eph, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);

            }


            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return  errs;
        }
        public static (List<MapSatInfo>, List<ErrDistanceMapPoints>) Gdop3SatDF(string mainLines, string adajLines, string adajLines2,
          DateTime captime, double fuHz1, double fuHz2, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
        {
            int satCount = 3;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            var timeSpan = (long)(captime - dtZero).TotalSeconds;
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop3SatDF(mainLines, adajLines, adajLines2, timeSpan, refPos, fuHz1, fuHz2, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);

            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<MapSatInfo> satInfos = ParseResult(satCount, satllh, mainLines, adajLines, adajLines2);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return (satInfos, errs);
        }

        public static List<ErrDistanceMapPoints> Gdop3SatDFByXyz(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
          double fuHz1, double fuHz2, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
        {
            int satCount = 3;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

           
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop3SatDFByXyz(mainEph, adaj1Eph, adaj2Eph, refPos, fuHz1, fuHz2, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);

            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return  errs;
        }

        public static (List<MapSatInfo>, List<ErrDistanceMapPoints>) Gdop2SatDRef(string mainLines, string adajLines,
      DateTime captime, double fuHz1, double fuHz2, double dtousErr, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
        {
            int satCount = 2;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];

            var timeSpan = (long)(captime - dtZero).TotalSeconds;
            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop2SatDRef(mainLines, adajLines, timeSpan, refPos, fuHz1, fuHz2, dtousErr, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);

            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<MapSatInfo> satInfos = ParseResult(satCount, satllh, mainLines, adajLines);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return (satInfos, errs);
        }

        public static List<ErrDistanceMapPoints> Gdop2SatDRefByXyz(double[] mainEph, double[] adajEph, double fuHz1, double fuHz2, double dtousErr, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
        {
            int satCount = 2;
            //该值和points 一一对应
            double[] level = GdopParam.误差配置.误差距离m;
            double[] satllh = new double[satCount * 3];


            IntPtr res = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop2SatDRefByXyz(mainEph, adajEph, refPos, fuHz1, fuHz2, dtousErr, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);

            IntPtr tmp = res;

            //用于绘制的数据
            List<double[]> points = new List<double[]>();
            for (int idx = 0; idx < level.Length; ++idx)
            {
                double[] levelval = new double[resCount[idx]];
                Marshal.Copy(tmp, levelval, 0, resCount[idx]);
                tmp += (resCount[idx] * sizeof(double));
                points.Add(levelval);
            }
            GDOPApi.FreeGDOPBuf(res);
            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
            for (int i = 0; i < points.Count; i++)
            {
                if (!points[i].Any()) continue;
                ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                errDistanceMap.ErrDistance = level[i];
                errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
                errs.Add(errDistanceMap);
            }
            return  errs;
        }
        private static List<MapDot> ParseResult(double[] ponits)
        {
            List<MapDot> mapDots = new List<MapDot>();
            int count = 2;
            for (int i = 0; i < ponits.Length / count; i++)
            {
                MapDot mapDot = new MapDot();
                mapDot.Lon = ponits[count * i];
                mapDot.Lat = ponits[count * i + 1];//0 1  2  3  4 5 6 7
                mapDots.Add(mapDot);
            }
            return mapDots;

        }

       
        private static List<MapSatInfo> ParseResult(int satCount, double[] satllh, params string[] ephLine)
        {

            List<MapSatInfo> list = new List<MapSatInfo>();
            int len = 3;
            for (int i = 0; i < satCount; i++)
            {
                MapSatInfo satInfo = new MapSatInfo();
                var ephstrs = ephLine[i].Split(new string[] { " ", "U" }, StringSplitOptions.RemoveEmptyEntries);
                if (ephstrs.Length == 16)
                {
                    satInfo.SatCode = Convert.ToInt32(ephstrs[1]);
                }
                satInfo.SatLon = Convert.ToDouble(satllh[len * i]);
                satInfo.SatLat = Convert.ToDouble(satllh[len * i + 1]);
                list.Add(satInfo);
            }

            return list;
        }
    }

}