DW5S/DW5S.KxcApi/AddIns/GDOP误差椭圆/GdopHelper.cs


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

namespace DW5S.KxcApi
{
    public static class GdopHelper
    {

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


        public static double[] ErrorEllipse2X1D(double posLon, double posLat, double[] mainEph, double[] adajEph, double[] cdbPos, double[] RefGeod, double DtoErrus, double EphErrm)
        {
            IEnumerable<double> res = new List<double>();
            int LOP_Len = 0;
            /// <summary>
            /// 概率 默认0.5
            /// </summary>
            double Pe = 0.5;
            IntPtr LOP_ValuePtr = GDOPApi.Error_Ellipse_2X1D(
                   mainEph,
                   adajEph,
                   cdbPos,
                   RefGeod,
                    new double[3] { posLon, posLat, 0 },
                    DtoErrus * 1e-6,
                    EphErrm,//单位m
                    Pe, ref LOP_Len);
            List<DtoLinePoint> list = new List<DtoLinePoint>();
            double[] LOP_Value = new double[LOP_Len];
            if (LOP_Len > 0)
            {
                Marshal.Copy(LOP_ValuePtr, LOP_Value, 0, LOP_Value.Length);

                int lastcount = LOP_Len - 3;
                res = LOP_Value.Skip(lastcount).Take(3);
            }
            return res.ToArray();
        }


        /// <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="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> Gdop2Sat1DByXyzNew(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;
            IntPtr lpoints = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.Gdop2Sat1DNoRefByXyz_new(mainEph, adajEph, cdbPos, dtousErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);

            }
            else
            {

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

            }

            var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
            GDOPApi.FreeGDOPBuf(res);
            GDOPApi.FreeGDOPBuf(lpoints);
            return errs;
        }
        private static List<ErrDistanceMapPoints> ToErrDistanceMapPoints(double[] level, int[] resCount, IntPtr lpoints, IntPtr res)
        {
            int total = resCount.Sum(r => r);
            int[] Points_Value = new int[total];
            Marshal.Copy(lpoints, Points_Value, 0, Points_Value.Length);

            int totalPoint = Points_Value.Sum(p => p);
            double[] LOP_Value = new double[totalPoint * 2];
            Marshal.Copy(res, LOP_Value, 0, LOP_Value.Length);

            var points = ParseResult(LOP_Value);
            //用于绘制的数据

            List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();

            int skippointcount = 0;
            int skipcount = 0;
            int count = 0;
            for (int idx = 0; idx < level.Length; ++idx)
            {
                int levelcount = resCount[idx];
                skipcount = idx == 0 ? 0 : skipcount + resCount[idx - 1];
                for (int i = skipcount; i < levelcount + skipcount; i++)
                {
                    int pointcount = Points_Value[i];
                    skippointcount = count == 0 ? 0 : skippointcount + Points_Value[i - 1];
                    var mapDots = points.Skip(skippointcount).Take(pointcount);
                    if (!mapDots.Any()) continue;
                    ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
                    errDistanceMap.ErrDistance = level[idx];
                    errDistanceMap.MapDots.AddRange(mapDots);
                    errs.Add(errDistanceMap);
                    count++;
                }

            }

            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<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<ErrDistanceMapPoints> Gdop1Sat1DByXyzNew(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;
            IntPtr lpoints = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            if (refPos == null || refPos.Length == 0)
            {
                GDOPApi.GdopXDCXNoRefByXyz_new(mainEph, cdbPos, cxPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);
            }
            else
            {
                GDOPApi.GdopXDCXRefByXyz_new(mainEph, cdbPos, cxPos, refPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);
            }
            var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
            GDOPApi.FreeGDOPBuf(res);
            GDOPApi.FreeGDOPBuf(lpoints);
            return 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<ErrDistanceMapPoints> Gdop3SatByXyzNew(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;

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

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

            }
            var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
            GDOPApi.FreeGDOPBuf(res);
            GDOPApi.FreeGDOPBuf(lpoints);
            return 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<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<(double lon, double lat)> ParseResult(double[] ponits)
        {
            List<(double lon, double lat)> mapDots = new List<(double lon, double lat)>();
            int count = 2;
            for (int i = 0; i < ponits.Length / count; i++)
            {
                var Lon = ponits[count * i];
                var Lat = ponits[count * i + 1];//0 1  2  3  4 5 6 7
                mapDots.Add((Lon, Lat));
            }
            return mapDots;

        }


        public static List<ErrDistanceMapPoints> Gdop3SatDFByXyzNew(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;
            IntPtr lpoints = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop3SatDFByXyz_new(mainEph, adaj1Eph, adaj2Eph, refPos, fuHz1, fuHz2, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out lpoints, out res, satllh);

            var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
            GDOPApi.FreeGDOPBuf(res);
            GDOPApi.FreeGDOPBuf(lpoints);
            return errs;
        }

        public static List<ErrDistanceMapPoints> Gdop2SatDRefByXyzNew(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;
            IntPtr lpoints = IntPtr.Zero;
            int[] resCount = new int[level.Length];
            GDOPApi.Gdop2SatDRefByXyz_new(mainEph, adajEph, refPos, fuHz1, fuHz2, dtousErr, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out lpoints, out res, satllh);
            var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
            GDOPApi.FreeGDOPBuf(res);
            GDOPApi.FreeGDOPBuf(lpoints);
            return errs;
        }

    }

}