package org.springblade.business.utils;




import org.springblade.business.entity.Mileage;
import org.springblade.business.entity.Mileagexy;

import java.util.ArrayList;
import java.util.List;


public class MileageUtill {
    private static final String prefix = "K";
    private static final int precision = 8;
    private static final int precision_1 = 3;
    private static final int spacing_precision = 3;
    private static final double spacing = 0.001;
    /**主线*/
    public static final String MASTER_PART = "NO1";


    /******************交点法***********开始
     /**
     * 初始化里程信息数据
     * @param mileages
     * @return
     */
    public static List<Mileage> initMileages(List<Mileage> mileages) {
        if (mileages == null && mileages.size() < 2) {
            return mileages;
        }

        for (int i = 0; i < mileages.size() - 1; i++) {
            //首先点到的点长 和 方位角
            initMileage(mileages.get(i), mileages.get(i + 1));
        }
        for (int i = 0; i < mileages.size() - 1; i++) {
            //再计算曲线要素及主点坐标
            initMileage2(mileages.get(i), mileages.get(i + 1));
        }
        //优化曲线首尾相连
        initMileage4(mileages);

        return mileages;
    }

    /**
     * 计算坐标
     */
    public static double[] azimuthZb(double lc, double b,double jj, List<Mileage> mileages) {
        double[] res = null;
        List<Mileage> list=null;
        for(int i=1;i<10;i++) {
           list=new ArrayList<>();
            String partNo="NO"+i;
            for(Mileage m:mileages){
                if(partNo.equals(m.getPartNo())){
                    list.add(m);
                }
            }
            if(list.size()>2){
                res=azimuthZbNew(lc,b,jj,list);
               if(res!=null){
                   break;
               }
            }

        }
        return res;
    }
    /**
     * 计算坐标
     */
    private static double[] azimuthZbNew(double lc, double b,double jj, List<Mileage> mileages) {
        double[] res = null;
        double[] zzres = null;
        double[] xy;
        double l;
        int type = 0;
        double A;

        for (int i = 1; i < mileages.size() - 1; i++) {

            Mileage q = mileages.get(i - 1);
            Mileage j = mileages.get(i);
            Mileage z = mileages.get(i + 1);
            if (j.getSwing().equals("左转")) {
                type = 1;
            } else {
                type = 0;
            }
            double qd = 0;
            double qx = 0, qy = 0;
            if (i == 1) {
                qd = q.getValue();
                qx = q.getX();
                qy = q.getY();
            } else {
                qd = q.getHz();
                qx = q.getHzx();
                qy = q.getHzy();
            }
            double zd = z.getValue();
            if (qd < lc && lc <= j.getZh()) {//在起点至zh上(直线上)


                    zzres = azimuthZb(qx, qy, lc - qd, q.getAngle());
                A = q.getAngle() + jj;
                res = azimuthZb(zzres[0], zzres[1], b, A);
//                    if (b > 0) {
//                        A = q.getAngle() + jj;
//                    } else {
//                        A = q.getAngle() - jj;
//                    }
//                    if (A >= 360) {
//                        A = A - 360;
//                    }
//                    if (A < 0) {
//                        A = A + 360;
//                    }
//
//                    if (b > 0) {
//                        res = azimuthZb(zzres[0], zzres[1], b, A);
//                    } else {
//                        res = azimuthZb(zzres[0], zzres[1], -b, A);
//                    }

            } else if (j.getZh() < lc && lc <= j.getHy()) {//在zh至hy上(第一缓和线上)
                xy = azimuthZbhhqx1(j.getR(), lc - j.getZh(), j.getLs1());

                    l = lc - j.getZh();
                    double b01 = (l * l / (2 * j.getR() * j.getLs1())) * (180 / Math.PI);
                    if (type == 1) {
                        b01 = -b01;
                    }
//                    if (b > 0) {
//                        A = q.getAngle() + b01 + jj;
//                    } else {
//                        A = q.getAngle() + b01 - jj;
//                    }
//                    if (A >= 360) {
//                        A = A - 360;
//                    }
//                    if (A < 0) {
//                        A = A + 360;
//                    }

                    zzres = azimuthZbqx(j.getZhx(), j.getZhy(), q.getAngle(), xy[0], xy[1], type);
                A = q.getAngle() + b01 + jj;
                res = azimuthZb(zzres[0], zzres[1], b, A);
//                    if (b > 0) {
//                        res = azimuthZb(zzres[0], zzres[1], b, A);
//                    } else {
//                        res = azimuthZb(zzres[0], zzres[1], -b, A);
//                    }

            } else if (j.getHy() < lc && lc <= j.getYh()) {//在hy至yh上(圆曲线上)
                xy = azimuthZbyqx(j.getR(), lc - j.getZh(), j.getLs1());

                    l = lc - j.getZh();

                    double g = ((l - j.getLs1()) / j.getR() + j.getB01()) * (180 / Math.PI);

                    if (type == 1) {
                        g = -g;
                    }
//                    if (b > 0) {
//                        A = q.getAngle() + g + jj;
//                    } else {
//                        A = q.getAngle() + g - jj;
//                    }
//                    if (A >= 360) {
//                        A = A - 360;
//                    }
//                    if (A < 0) {
//                        A = A + 360;
//                    }
                    zzres = azimuthZbqx(j.getZhx(), j.getZhy(), q.getAngle(), xy[0], xy[1], type);
                A = q.getAngle() + g + jj;
                res = azimuthZb(zzres[0], zzres[1], b, A);
//                    if (b > 0) {
//                        res = azimuthZb(zzres[0], zzres[1], b, A);
//                    } else {
//                        res = azimuthZb(zzres[0], zzres[1], -b, A);
//                    }


//            }else if(j.getHy()<lc&&lc<=j.getQz()) {//在hy至qz上(圆曲线上)
//            }else if(j.getQz()<lc&&lc<=j.getYh()) {//在qz至yh上(圆曲线上)
//                }
//

            } else if (j.getYh() < lc && lc <= j.getHz()) {//在yh至hz上(第二缓和线上)
                xy = azimuthZbhhqx1(j.getR(), lc - j.getHz(), j.getLs2());

                    l = lc - j.getHz();
                    // double b01=l/(2*j.getR())*(180/Math.PI);
                    double b01 = (l * l / (2 * j.getR() * j.getLs2())) * (180 / Math.PI);
                    if (type == 1) {
                        b01 = -b01;
                    }

//                    if (b > 0) {
//                        A = j.getAngle() - b01 + jj;
//                    } else {
//                        A = j.getAngle() - b01 - jj;
//                    }
//                    if (A >= 360) {
//                        A = A - 360;
//                    }
//                    if (A < 0) {
//                        A = A + 360;
//                    }
                    zzres = azimuthZbqx(j.getHzx(), j.getHzy(), j.getAngle(), xy[0], -xy[1], type);
                A = j.getAngle() - b01 + jj;
                res = azimuthZb(zzres[0], zzres[1], b, A);
//                    if (b > 0) {
//                        res = azimuthZb(zzres[0], zzres[1], b, A);
//                    } else {
//                        res = azimuthZb(zzres[0], zzres[1], -b, A);
//                    }


            } else if (j.getHz() < lc && lc <= zd) {//在hz至终点上(直线上)
                if(j.getDlq()!=0&&lc>j.getDlq()){
                    lc=lc-(j.getDlh()-j.getDlq());
                }

                    zzres = azimuthZb(j.getHzx(), j.getHzy(), lc - j.getHz(), j.getAngle());
                A = j.getAngle() + jj;
                res = azimuthZb(zzres[0], zzres[1], b, A);
//                    if (b > 0) {
//                        A = j.getAngle() + jj;
//                    } else {
//                        A = j.getAngle() - jj;
//                    }
//                    if (A > 360) {
//                        A = A - 360;
//                    }
//                    if (A < 0) {
//                        A = A + 360;
//                    }
//                    if (b > 0) {
//                        res = azimuthZb(zzres[0], zzres[1], b, A);
//                    } else {
//                        res = azimuthZb(zzres[0], zzres[1], -b, A);
//                    }
                }

        }
        return res;
    }


    /**
     * 初始化里程数据
     */
    private static void initMileage0(Mileage mileage) {
        int type = mileage.getType();
        Double value = mileage.getValue();
        Double x = mileage.getX();
        Double y = mileage.getY();
        Integer orderNum = mileage.getOrderNum();
        String code = mileage.getCode();
        Double r = mileage.getR();
        Double ls1 = mileage.getLs1();
        Double ls2 = mileage.getLs2();
        Double dlq = mileage.getDlq();
        Double dlh = mileage.getDlh();

        if (type == 1) {
            mileage = new Mileage(code, x, y, r, orderNum, type, ls1, ls2, dlq, dlh);
        } else {
            if (type == 2) {
                value = 0.0;
            }
            mileage = new Mileage(value, x, y, orderNum, type);
        }
    }

    /**
     * 初始化里程数据 点到点的长，方位角，交点坐标
     */
    private static void initMileage(Mileage mileage, Mileage nextMileage) {

        double x1 = mileage.getX();
        double y1 = mileage.getY();
        double x2 = nextMileage.getX();
        double y2 = nextMileage.getY();


        double dx, dy, angle = 0;
        dx = x2 - x1;
        dy = y2 - y1;


        //计算方位角
        double yx = dy / dx;
        if (yx < 0) {
            yx = -yx;
        }

        angle = Math.atan(yx);
        angle = angle * (180 / Math.PI);
        if (dx > 0 && dy >= 0) {
        } else if (dx < 0 && dy >= 0) {
            angle = 180 - angle;
        } else if (dx < 0 && dy < 0) {
            angle = 180 + angle;
        } else if (dx > 0 && dy < 0) {
            angle = 360 - angle;
        }else if (dx == 0 && dy > 0) {
            angle = 90;
        }else if (dx == 0 && dy < 0) {
            angle = 270;
        }

        if(angle!=0) {
            angle = NumberUtil.round(angle, precision);
        }
        mileage.setAngle(angle);

        //计算点到点的距离
        if (dx < 0) {
            dx = -dx;
        }
        if (dy < 0) {
            dy = -dy;
        }
        double l = Math.sqrt(dx * dx + dy * dy);
        l = NumberUtil.round(l, precision);
        nextMileage.setLjd(l);
        nextMileage.setValue(mileage.getValue() + l);
//        nextMileage.setValue(mileage.getValue()+NumberUtil.round(l,precision));

    }

    /**
     * 初始化里程数据 曲线要素，转角
     */
    private static void initMileage2(Mileage mileage, Mileage nextMileage) {
        double ACorner=nextMileage.getAngle() - mileage.getAngle();

        if(ACorner>180){
            ACorner= ACorner-360;
        }
        if(-ACorner>180){
            ACorner= ACorner+360;
        }
        nextMileage.setCorner(ACorner);

        double R = mileage.getR();
        double ls1 = mileage.getLs1();
        double ls2 = mileage.getLs2();

        double A = mileage.getCorner();
        double T1 = 0, L = 0, D = 0, Lc = 0, T2 = 0, b01 = 0, b02 = 0, p1 = 0, q1 = 0, p2 = 0, q2 = 0;

        //计算曲线要素
        /**
         *   //书本算法

         b01=ls1/(2*R);
         b02=ls2/(2*R);
         p1=ls1*ls1/(24*R);
         p2=ls2*ls2/(24*R);
         q1=(ls1/2)-(ls1*ls1*ls1/(240*R*R));
         q2=(ls2/2)-ls2*ls2*ls2/(240*R*R);

         if(A<0){
         A=-A;
         mileage.setSwing("左转");
         }else{
         mileage.setSwing("右转");
         }
         mileage.setCorner(A);
         A=A* Math.PI/180;
         T1=(R+p1)*Math.tan(A/2)+q1;
         T1= NumberUtil.round(T1,precision);
         T2=(R+p2)*Math.tan(A/2)+q2;
         T2= NumberUtil.round(T2,precision);
         */
        if (A < 0) {
            A = -A;
            mileage.setSwing("左转");
        } else {
            mileage.setSwing("右转");
        }
        mileage.setCorner(A);
        if (R != 0) {

            b01 = ls1 / (2 * R);
            b01 = NumberUtil.round(b01, precision);
            b02 = ls2 / (2 * R);
            b02 = NumberUtil.round(b02, precision);
            p1 = ls1 / 2 - (Math.pow(ls1, 3) / (240 * R * R)) + (Math.pow(ls1, 5) / (34560 * Math.pow(R, 4)));
            p1 = NumberUtil.round(p1, precision);
            p2 = ls2 / 2 - (Math.pow(ls2, 3) / (240 * R * R)) + (Math.pow(ls2, 5) / (34560 * Math.pow(R, 4)));
            p2 = NumberUtil.round(p2, precision);
            q1 = Math.pow(ls1, 2) / (24 * R) - (Math.pow(ls1, 4) / (2688 * Math.pow(R, 3))) + (Math.pow(ls1, 6) / (506880 * Math.pow(R, 5)));
            q1 = NumberUtil.round(q1, precision);
            q2 = Math.pow(ls2, 2) / (24 * R) - (Math.pow(ls2, 4) / (2688 * Math.pow(R, 3))) + (Math.pow(ls2, 6) / (506880 * Math.pow(R, 5)));
            q2 = NumberUtil.round(q2, precision);


            A = A * Math.PI / 180;
            T1 = p1 + (R + q2 - (R + q1) * Math.cos(A)) / Math.sin(A);
            T1 = NumberUtil.round(T1, precision);
            T2 = p2 + (R + q1 - (R + q2) * Math.cos(A)) / Math.sin(A);
            T2 = NumberUtil.round(T2, precision);
            if (T1 < 0) {
                T1 = -T1;
            }
            if (T2 < 0) {
                T2 = -T2;
            }
            Lc = R * (A - b01 - b02);
//        Lc=R*A-(ls1/2)-(ls2/2);
            Lc = NumberUtil.round(Lc, precision);
            L = Lc + ls1 + ls2;
            L = NumberUtil.round(L, precision);

            D = T1 + T2 - L;
            D = NumberUtil.round(D, precision);
        }
        mileage.setT1(T1);
        mileage.setT2(T2);
        mileage.setB01(b01);
        mileage.setB02(b02);
        mileage.setQ1(q1);
        mileage.setQ2(q2);
        mileage.setP1(p1);
        mileage.setP2(p2);
        mileage.setL(L);
        mileage.setLc(Lc);
        mileage.setD(D);
        initMileage3(mileage, nextMileage);
    }

    /****
     * 计算主点里程  计算下一交点（终点）里程桩号
     * @param mileage
     * @param nextMileage
     */

    private static void initMileage3(Mileage mileage, Mileage nextMileage) {
        //计算主点里程
        double hy = 0, yh = 0, hz = 0, qz = 0, dl = 0;
        double zh = mileage.getValue() - mileage.getT1();
        dl = mileage.getDlh() - mileage.getDlq();
        zh = NumberUtil.round(zh, precision);
        hy = zh + mileage.getLs1();
        hy = NumberUtil.round(hy, precision);
        yh = hy + mileage.getLc();
        yh = NumberUtil.round(yh, precision);
        hz = yh + mileage.getLs2();
        hz = NumberUtil.round(hz, precision);
        qz = hz - mileage.getL() / 2;
        //计算下一交点（终点）里程桩号
        double nextValue = hz + nextMileage.getLjd() - mileage.getT2() + dl;
        mileage.setZh(zh);
        mileage.setHy(hy);
        mileage.setYh(yh);
        mileage.setHz(hz);
        mileage.setQz(qz);

//        nextValue= NumberUtil.round(nextValue,precision);
        nextMileage.setValue(nextValue);

    }

    /***
     * 计算zh,hz的坐标
     * @param mileages
     */
    private static void initMileage4(List<Mileage> mileages) {
        for (int i = 1; i < mileages.size() - 1; i++) {
            double[] xy = new double[2];
            Mileage q = mileages.get(i - 1);
            Mileage j = mileages.get(i);
            //去掉小于spacing的首尾相连
            double cj;
            cj = j.getZh() - q.getHz();
            cj = NumberUtil.round(cj, spacing_precision);
            if (cj < 0) {
                cj = -cj;
            }
            if (cj < spacing) {
                j.setValue(q.getHz() + j.getT1());

                for (int ii = i; ii < mileages.size() - 1; ii++) {
                    initMileage3(mileages.get(ii), mileages.get(ii + 1));
                }

            }
            if (j.getType() == 1) {


                xy = azimuthZb(j.getX(), j.getY(), -j.getT1(), q.getAngle());
                j.setZhx(xy[0]);
                j.setZhy(xy[1]);

                xy = azimuthZb(j.getX(), j.getY(), j.getT2(), j.getAngle());
                j.setHzx(xy[0]);
                j.setHzy(xy[1]);
            }
        }
    }
    /**************交点法******结束***/

    /******************线元法***********开始**/
    /**
     * 初始化线元里程信息数据
     *
     * @param mileagexys
     * @return
     */
    public static List<Mileagexy> initMileagexys(List<Mileagexy> mileagexys, int oderNum) {
        for (int i = 0; i < mileagexys.size() - 1; i++) {
            if (mileagexys.get(i).getOrderNum() >= oderNum) {
                initMileagexy(mileagexys.get(i), mileagexys.get(i + 1));
            }
        }
        return mileagexys;
    }

    /**
     * 线元法坐标计算
     * @param lc
     * @param b
     * @param jj
     * @param mileagexys
     * @return
     */
    public static double[] azimuthZbXy(double lc, double b,double jj, List<Mileagexy> mileagexys) {
        double[] res = null;

        for (Mileagexy mileagexy : mileagexys) {
            if (mileagexy.getValue() <= lc && (mileagexy.getValueEnd()+0.0000000000001) >= lc) {
                double angle=mileagexy.getAngle();
                double l = lc - mileagexy.getValue();
                int type = mileagexy.getType();
                int sw = 0;
                if ("左转".equals(mileagexy.getSwing())) {
                    sw = 1;
                } else {
                    sw = 0;
                }
                if (type == 0) {
                    if(mileagexy.getDlq()!=0&&lc>mileagexy.getDlq()){
                           l=lc-(mileagexy.getDlh()-mileagexy.getDlq())- mileagexy.getValue();
                    }
                    res = azimuthZb(mileagexy.getX(), mileagexy.getY(), l, mileagexy.getAngle());

                } else if (type == 1) {
                    if(mileagexy.getDlq()!=0&&lc>mileagexy.getDlq()){
                        l=lc-(mileagexy.getDlh()-mileagexy.getDlq())- mileagexy.getValue();
                    }
                    double[] xy = azimuthZbyqx(mileagexy.getR(), l, 0);
                    double g = (l / mileagexy.getR()) * (180 / Math.PI);
                    res = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), mileagexy.getAngle(), xy[0], xy[1], sw);
                    if (sw == 1) {
                        g = -g;
                    }
                    angle=angle+g;

                } else if (type == 2) {
                    if(mileagexy.getDlq()!=0&&lc>mileagexy.getDlq()){
                        l=lc-(mileagexy.getDlh()-mileagexy.getDlq())- mileagexy.getValue();
                    }
                    double b01 = (l * l / (2 * mileagexy.getR() * mileagexy.getL())) * (180 / Math.PI);
                    double[] xy = azimuthZbhhqx1(mileagexy.getR(), l, mileagexy.getL());
                    res = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), mileagexy.getAngle(), xy[0], xy[1], sw);
                    if (sw == 1) {
                        b01 = -b01;
                    }
                    angle=angle+b01;

                } else if (type == 3) {
                    if(mileagexy.getDlq()!=0&&lc>mileagexy.getDlq()){
                        l=lc-(mileagexy.getDlh()-mileagexy.getDlq())- mileagexy.getValueEnd();
                    }else {
                    l =   lc-mileagexy.getValueEnd();
                    }
                    double[] xy1 = azimuthZbhhqx1(mileagexy.getR(), l, mileagexy.getL());
                    double[] xy2 = azimuthZbhhqx1(mileagexy.getR(), mileagexy.getL(), mileagexy.getL());
                    double b03 = (l * l / (2 * mileagexy.getR() * mileagexy.getL())) * (180 / Math.PI);
                    double b02 = (mileagexy.getL() / (2 * mileagexy.getR())) * (180 / Math.PI);
                    double a = mileagexy.getAngle() + b02;
                    if(sw==1){
                        a = mileagexy.getAngle() - b02;
                        b03= -b03;
                    }
                    double[] resend = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), a, xy2[0], -xy2[1], sw);
                    res = azimuthZbqx(resend[0], resend[1], a, xy1[0], -xy1[1], sw);
                    angle=a - b03;

                } else if (type == 4) {
                    double c = (mileagexy.getL() * mileagexy.getR() * mileagexy.getR2()) / (mileagexy.getR() - mileagexy.getR2());
                    double ls = c / mileagexy.getR2();
                    double lp = ls - mileagexy.getL();
                    l = l + lp;
                    double[] xy = azimuthZbhhqx1((mileagexy.getR2()), l, ls);
                    double b03 = ((lp * lp) / (2 * mileagexy.getR2() * ls)) * (180 / Math.PI);
                    double b01 = ((l * l) / (2 * mileagexy.getR2() * ls)) * (180 / Math.PI);
                    if (sw == 1) {
                        b01 = -b01;
                        b03 = -b03;
                    }
                    double[] hzres;
                    double[] hzxy = azimuthZbhhqx1((mileagexy.getR2()), lp, ls);
                    hzres = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), mileagexy.getAngle() - b03, -hzxy[0], -hzxy[1], sw);
                    res = azimuthZbqx(hzres[0], hzres[1], mileagexy.getAngle() - b03, xy[0], xy[1], sw);
                    angle=angle- b03 + b01;
                }
                if (res != null) {
                     res = azimuthZb(res[0], res[1], b, angle + jj);

                }
            }
        }

        return res;
    }

    public static void initMileagexy(Mileagexy mileagexy, Mileagexy nextMileagexy) {
        initMileagexyValueEnd(mileagexy);
        int type = mileagexy.getType();

        int sw = 0;
        if ("左转".equals(mileagexy.getSwing())) {
            sw = 1;
        } else {
            sw = 0;
        }
        if (type == 0) {

            double[] res = azimuthZb(mileagexy.getX(), mileagexy.getY(), mileagexy.getL(), mileagexy.getAngle());
            nextMileagexy.setValue(mileagexy.getValueEnd());
            nextMileagexy.setX(NumberUtil.round(res[0], precision));
            nextMileagexy.setY(NumberUtil.round(res[1], precision));
            initMileagexyAngle(nextMileagexy,mileagexy.getAngle());
            nextMileagexy.setOrderNum(mileagexy.getOrderNum() + 1);
            initMileagexyValueEnd(nextMileagexy);

        } else if (type == 2 || type == 3) {
            double[] res;
            double[] xy = azimuthZbhhqx1(mileagexy.getR(), mileagexy.getL(), mileagexy.getL());
            // double b01=(mileagexy.getL()/(2*mileagexy.getR()))*(180/Math.PI);
            double b01 = ((mileagexy.getL() * mileagexy.getL()) / (2 * mileagexy.getR() * mileagexy.getL())) * (180 / Math.PI);
            if (type == 2) {
                res = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), mileagexy.getAngle(), xy[0], xy[1], sw);
            } else {
                double a = mileagexy.getAngle() + b01;
                if(sw==1){
                    a = mileagexy.getAngle() - b01;
                }
//                    String[] ang=convertToSexagesimal1(a);
//                    a=Double.valueOf(ang[0])+(Double.valueOf(ang[1])/60)+(Double.valueOf(ang[2])/3600);
                res = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), a, xy[0], -xy[1], sw);
            }
            nextMileagexy.setValue(mileagexy.getValueEnd());
            nextMileagexy.setX(NumberUtil.round(res[0], precision));
            nextMileagexy.setY(NumberUtil.round(res[1], precision));


            if (sw == 1) {
                b01 = -b01;
            }
            double angle=(mileagexy.getAngle() + b01);
            initMileagexyAngle(nextMileagexy,angle);
            nextMileagexy.setOrderNum(mileagexy.getOrderNum() + 1);
            initMileagexyValueEnd(nextMileagexy);
        } else if (type == 4) {
            double[] res;
            double c = (mileagexy.getL() * mileagexy.getR() * mileagexy.getR2()) / (mileagexy.getR() - mileagexy.getR2());
            double l = c / mileagexy.getR2();
            double lp = l - mileagexy.getL();
            double[] xy = azimuthZbhhqx1((mileagexy.getR2()), l, l);
            double b02 = (l / (2 * (mileagexy.getR2()))) * (180 / Math.PI);
            double b03 = ((lp * lp) / (2 * mileagexy.getR2() * l)) * (180 / Math.PI);
            double b01 = b02 - b03;
            double[] hzres;
            double[] hzxy = azimuthZbhhqx1((mileagexy.getR2()), lp, l);
            double ang=mileagexy.getAngle() - b03;
            if (sw == 1) {
                ang=mileagexy.getAngle() +b03;
            }
            hzres = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), ang, -hzxy[0], -hzxy[1], sw);
            res = azimuthZbqx(hzres[0], hzres[1], ang, xy[0], xy[1], sw);
            nextMileagexy.setValue(mileagexy.getValueEnd());
            nextMileagexy.setX(NumberUtil.round(res[0], precision));
            nextMileagexy.setY(NumberUtil.round(res[1], precision));


            if (sw == 1) {
                b01 = -b01;
            }
            double angle=(mileagexy.getAngle() + b01);
            initMileagexyAngle(nextMileagexy,angle);
            nextMileagexy.setOrderNum(mileagexy.getOrderNum() + 1);
            initMileagexyValueEnd(nextMileagexy);
        } else if (type == 1) {
            double[] xy = azimuthZbyqx(mileagexy.getR(), mileagexy.getL(), 0);
            double[] res = azimuthZbqx(mileagexy.getX(), mileagexy.getY(), mileagexy.getAngle(), xy[0], xy[1], sw);
            nextMileagexy.setValue(mileagexy.getValueEnd());
            nextMileagexy.setX(NumberUtil.round(res[0], precision));
            nextMileagexy.setY(NumberUtil.round(res[1], precision));
            double g = (mileagexy.getL() / mileagexy.getR()) * (180 / Math.PI);
            if (sw == 1) {
                g = -g;
            }
            double angle=(mileagexy.getAngle() + g);
            initMileagexyAngle(nextMileagexy,angle);
            nextMileagexy.setOrderNum(mileagexy.getOrderNum() + 1);
            initMileagexyValueEnd(nextMileagexy);
        }

    }

    public static void initMileagexyAngle(Mileagexy nextMileagexy,double angle) {
        if (angle >= 360) {
            angle = angle - 360;
        }
        if (angle < 0) {
            angle = angle + 360;
        }
        String[] ang = convertToSexagesimal1(angle);
        if(nextMileagexy.getType()==0) {
            if(nextMileagexy.getAngle()==0.0) {
                nextMileagexy.setAngle(angle);
                nextMileagexy.setDu(Integer.valueOf(ang[0]));
                nextMileagexy.setFen(Integer.valueOf(ang[1]));
                nextMileagexy.setMiao(Double.valueOf(ang[2]));
            }
        }else{
            nextMileagexy.setAngle(angle);
            nextMileagexy.setDu(Integer.valueOf(ang[0]));
            nextMileagexy.setFen(Integer.valueOf(ang[1]));
            nextMileagexy.setMiao(Double.valueOf(ang[2]));
        }
    }

    public static void initMileagexyValueEnd(Mileagexy mileagexy) {
        double dl=0.0;
//        if(mileagexy.getType()==0){
            dl=mileagexy.getDlh()-mileagexy.getDlq();
//        }
        mileagexy.setValueEnd(mileagexy.getValue()+mileagexy.getL()+dl);
        double fen = Double.valueOf(mileagexy.getFen());
        fen=fen/60;
        double miao = mileagexy.getMiao() / 3600;
        mileagexy.setAngle(mileagexy.getDu()+fen+miao);
}

    /**************线元法******结束***/

    /**计算直线坐标
     *
     */
    private static double[] azimuthZb(double x1, double y1,double d,double a){

        double x=0,y=0;
        double[] res=new double[3];
        a=a* Math.PI/180;
        x= x1+d*Math.cos(a);
        //  x= NumberUtil.round(x,precision);
        y= y1+d*Math.sin(a);
        // y= NumberUtil.round(y,precision);
        // System.out.println(NumberUtil.format(y,3));
        res[0]=x;
        res[1]=y;
        res[2]=a/(Math.PI/180)-90 ;

        return res;
    }
    /**计算曲线坐标
     *8-1-23
     */
    private static double[] azimuthZbqx(double x1, double y1,double a,double xx,double yy,int type) {
        double x = 0, y = 0;
        double[] res = new double[2];
        a = a * Math.PI / 180;
        if (type==1) {
            yy=-yy;
        }
        x = x1 + xx * Math.cos(a) - yy * Math.sin(a);
        // x = NumberUtil.round(x, precision);

        y = y1 + xx * Math.sin(a) + yy * Math.cos(a);

        // y= NumberUtil.round(y,precision);
        // System.out.println(NumberUtil.format(y,3));
        res[0]=x;
        res[1]=y;

        return res;
    }

    /**计算缓和曲线支距法坐标
     *
     */
    private static double[] azimuthZbhhqx(double r, double l,double ls){
        double x=0,y=0;
        double[] res=new double[2];

        x= l-Math.pow(l, 5)/(40*Math.pow(r, 2)*Math.pow(ls, 2));
        //   x= NumberUtil.round(x,precision);
        y= Math.pow(l, 3)/(6*r*ls);
        //  y= NumberUtil.round(y,precision);
        // System.out.println(NumberUtil.format(y,3));
        res[0]=x;
        res[1]=y;

        return res;
    }
    private static double[] azimuthZbhhqx1(double r, double l,double ls){
        double x=0,y=0;
        double[] res=new double[2];

        x= l-Math.pow(l, 5)/(40*Math.pow(r, 2)*Math.pow(ls, 2))+Math.pow(l, 9)/(3456*Math.pow(r, 4)*Math.pow(ls, 4));
        //   x= NumberUtil.round(x,precision);
        y= Math.pow(l, 3)/(6*r*ls)-Math.pow(l, 7)/(336*Math.pow(r, 3)*Math.pow(ls, 3))+Math.pow(l, 11)/(42240*Math.pow(r, 5)*Math.pow(ls, 5));
        //  y= NumberUtil.round(y,precision);
        // System.out.println(NumberUtil.format(y,3));
        res[0]=x;
        res[1]=y;

        return res;
    }
    /**计算圆曲线支距法坐标
     *
     */
    private static double[] azimuthZbyqx(double r, double l,double ls){
        double x=0,y=0,b0=0,p=0,q=0,g=0;
        double[] res=new double[2];
        b0=ls/(2*r);
        p=ls*ls/(24*r);
        q=ls/2-ls*ls*ls/(240*r*r);
        g=(l-ls)/r+b0;
        x= r*Math.sin(g)+q;
        // x= NumberUtil.round(x,precision);
        y= r*(1-Math.cos(g))+p;
        // y= NumberUtil.round(y,precision);
        // System.out.println(NumberUtil.format(y,3));
        res[0]=x;
        res[1]=y;

        return res;
    }

    /**
     *  转换度数
     */
    public static String convertToSexagesimal(double num){
        double numm=num;
        if(numm<0){
            numm=-numm;
        }
        int du=(int)Math.floor(Math.abs(numm));    //获取整数部分

        double temp=(numm-du)*60;

        int fen=(int)Math.floor(temp); //获取整数部分
        String fenstr=String.valueOf(fen);
        if(fen<10){
            fenstr="0"+String.valueOf(fen);
        }
        double miao=(temp-fen)*60;
        miao= NumberUtil.round(miao,2);
        String miaostr=String.valueOf(miao);
        if(miao<10){
            miaostr="0"+String.valueOf(miao);
        }
        if(num<0) {
            return "-" + du + "°" + fenstr + "′" + miaostr + "″";
        }

        return du+"°"+fenstr+"′"+miaostr+"″";



    }
    /**
     *  转换度数
     */
    public static String[] convertToSexagesimal1(double num){
        double numm=num;
        if(numm<0){
            numm=-numm;
        }
        int du=(int)Math.floor(Math.abs(numm));    //获取整数部分

        double temp=(numm-du)*60;

        int fen=(int)Math.floor(temp); //获取整数部分

        double miao=(temp-fen)*60;
        miao= NumberUtil.round(miao,4);

        String[] res=new String[3];
        res[0]=String.valueOf(du);
        res[1]=String.valueOf(fen);
        res[2]=String.valueOf(miao);
        return res;



    }


    /**
     *  转换里程
     */
    public static String convertToMileage(double num){
        double numm=num;
        if(numm<0){
            numm=-numm;
        }
        double numn = numm/1000;

        int du=(int)Math.floor(Math.abs(numn));    //获取整数部分

        double temp=(num-(du*1000));

        int fen=(int)Math.floor(temp); //获取整数部分
        String fenstr=String.valueOf(fen);
        if(fen<100){
            fenstr="0"+String.valueOf(fen);
        }
        if(fen<10){
            fenstr="00"+String.valueOf(fen);
        }

        int miao=(int)Math.floor((NumberUtil.round(temp-fen,precision_1))*Math.pow(10, precision_1)); //获取整数部分
        if(miao==0){
            if(num<0) {
                return "-"+prefix+du+"+"+fenstr;
            }
            return prefix+du+"+"+fenstr;
        }else{
            String miaotr=String.valueOf(miao);
            if(miao<100){
                miaotr="0"+String.valueOf(miao);
            }
            if(miao<10){
                miaotr="00"+String.valueOf(miao);
            }
            if(num<0) {
                return "-"+prefix+du+"+"+fenstr+"."+miaotr;
            }
            return prefix+du+"+"+fenstr+"."+miaotr;
        }




    }

    /**
     *
     * 生成逐桩坐标
     * @param lc 桩距
     * @param b  偏移
     * @param mileages
     * @return
     */
    public static List<Mileage> azimuthZzZb(double kslc,double jslc,double lc, double b,List<Mileage> mileages) {

        List<Mileage> relist=new ArrayList<>();
        if(mileages.size()<=2){
            return relist;
        }

        int qd=(int)(kslc/100);
        qd=qd*100;

        double l=(jslc-qd );
        if(l>0) {
            if (lc < l) {

                int c = (int) (l % lc == 0 ? (l / lc) : (l / lc) + 1);
                for (int ii = 0; ii < c; ii++) {
                    putZzMileages0(qd + (ii * lc), b, mileages, null, relist, 0);

                }

            }
        }
            for (Mileage mleage :mileages) {
               if(mleage.getType()==1){
                   if(mleage.getZh()>=kslc&&mleage.getZh()<=jslc) {
                       putZzMileages0(mleage.getZh(), b, mileages, null, relist, 0);
                   }
                   if(mleage.getHy()>=kslc&&mleage.getHy()<=jslc) {
                       putZzMileages0(mleage.getHy(),b,mileages,null,relist,0);
                   }
                   if(mleage.getYh()>=kslc&&mleage.getYh()<=jslc) {
                       putZzMileages0(mleage.getYh(),b,mileages,null,relist,0);
                   }
                   if(mleage.getHz()>=kslc&&mleage.getHz()<=jslc) {
                       putZzMileages0(mleage.getHz(),b,mileages,null,relist,0);
                   }
                   if(mleage.getQz()>=kslc&&mleage.getQz()<=jslc) {
                       putZzMileages0(mleage.getQz(),b,mileages,null,relist,0);
                   }

               }
            }
        putZzMileages0(jslc,b,mileages,null,relist,0);

        return relist;
    }
    /**
     * 线元法
     * 生成逐桩坐标
     * @param lc 桩距
     * @param b  偏移
     * @param mileagexys
     * @return
     */
    public static List<Mileage> azimuthZzZbXy(double kslc,double jslc,double lc, double b,List<Mileagexy> mileagexys) {
        List<Mileage> relist=new ArrayList<>();
        if(mileagexys.size()<1){
          return  relist;
        }
       // Collections.sort(mileagexys); // 按桩号排序
//        int qd=(int)(kslc/100);
//        qd=qd*100;
        double qd =kslc;
        double l=jslc-qd;
        if(l>0) {
            if (lc < l) {
                /**
                 * 生成桩点数量，如果不能整除则+1
                 */
                int c = (int) (l % lc == 0 ? (l / lc) : (l / lc) + 1);
                for (int ii = 0; ii < c; ii++) {
                    putZzMileages0(qd + (ii * lc), b, null, mileagexys, relist, 1);

                }

            }
        }

        for(Mileagexy mileagexy:mileagexys){
         if(mileagexy.getValue()>=kslc&&mileagexy.getValue()<=jslc) {
             putZzMileages0(mileagexy.getValue(), b, null, mileagexys, relist, 1);
         }

        }
        putZzMileages0(jslc, b, null, mileagexys, relist, 1);

        return relist;
    }
    private static void putZzMileages0(double lc,double b,List<Mileage> zhlist,List<Mileagexy> zhlistXY,List<Mileage> relist,int type){

        for(Mileage m:relist){
            if(lc==m.getValue()&&b==m.getB()){
                return;
            }
        }
        putZzMileages(lc, 0.0, zhlist, zhlistXY, relist, type);
        if (b != 0) {
            putZzMileages(lc, -b, zhlist, zhlistXY, relist, type);
            putZzMileages(lc, b, zhlist, zhlistXY, relist, type);
        }
    }

    private static void putZzMileages(double lc,double b,List<Mileage> zhlist,List<Mileagexy> zhlistXY,List<Mileage> relist,int type){

        double[] xy=null;
        if(type==0) {
            xy = azimuthZb(lc, b, 90, zhlist);
        }else{
            xy = azimuthZbXy(lc, b, 90, zhlistXY);
        }
        if(xy!=null) {
            Mileage zzMileage = new Mileage(lc, xy[0], xy[1], 0, 0);
            zzMileage.setB(b);
            relist.add(zzMileage);
        }
    }

    public static String[] azimuthLC(double x, double y,double jj, Mileage mileage,List<Mileage> mileages,List<Mileagexy> mileagexys,double ty) {
        String[] res = new String[2];
        double dx, dy,angle;
        dx = x - mileage.getX();
        dy = y -mileage.getY() ;
        //计算方位角
        double yx = dy / dx;
        double yx1 = dy / dx;
        if (yx < 0) {
            yx = -yx;
        }

        angle = Math.atan(yx);
        angle = angle * (180 / Math.PI);
        if (dx > 0 && dy >= 0) {
        } else if (dx < 0 && dy >= 0) {
            angle = 180 - angle;
        } else if (dx < 0 && dy < 0) {
            angle = 180 + angle;
        } else if (dx > 0 && dy < 0) {
            angle = 360 - angle;
        }else if (dx == 0 && dy > 0) {
            angle = 90;
        }else if (dx == 0 && dy < 0) {
            angle = 270;
        }
        double ab = Math.sqrt(dx * dx + dy * dy);
        double bc= dy/Math.sin((mileage.getAngle()+jj)* Math.PI/180);
        double bd= dx/Math.cos((mileage.getAngle()+jj)* Math.PI/180);

        double lcc= ab*Math.sin((angle-mileage.getAngle()-jj)* Math.PI/180);
        if((angle-mileage.getAngle()-jj)<ty){
            if(lcc<0){
                lcc=lcc*-1;
            }

        }else{
            if(ty<0&&lcc>0){
                lcc=lcc*-1;
            }
            if(ty>0&&lcc<0){
                lcc=lcc*-1;
            }
        }
        ty=angle-mileage.getAngle()-jj;
        double aaa=  NumberUtil.round(bd, 5)*100000-NumberUtil.round(bc, 5)*100000;
        if(aaa<0){
            aaa=-aaa;
        }
        double aa=  NumberUtil.round(ab, 5)*100000-NumberUtil.round(bc, 5)*100000;
        if(bc<0){
            aa= NumberUtil.round(ab, 5)*100000+NumberUtil.round(bc, 5)*100000;
        }

        if(aa<0){
        aa=-aa;
        }

        if((aa<10&&aaa<10)||(aaa==0)){
            res[0]= String.valueOf(NumberUtil.round(mileage.getValue(),4));
            if(bc>0&&bc<0.001){
                res[1]="0.0001";
            }else{
                res[1]= String.valueOf(NumberUtil.round(bc,4));
            }
    }else{
            if(ab>20){
                lcc=ab;
            }

            double  lc=NumberUtil.round(mileage.getValue(), 4)+lcc;
                double[] acm =null;
            if(mileages!=null){
                acm = azimuthZb(lc,0,jj,mileages)  ;
            }else{
                acm = azimuthZbXy(lc,0,jj,mileagexys)  ;
            }

                if(acm==null){
                return null;
                }
            Mileage m=new Mileage();
                m.setX(acm[0]);
                m.setY(acm[1]);
                m.setAngle(acm[2]);
                m.setValue(lc);

            res=  azimuthLC(x,y,jj,m,mileages,mileagexys,ty);

//

        }

        return res;
    }

    public static void main(String[] srt){
        Mileage mm=new Mileage();
        mm.setAngle(30.331496472222224);
        mm.setX(2864425.47798996);
        mm.setY(37385681.63883557);
        mm.setValue(5531.690890000001);

        List<Mileagexy> mileagexys =new ArrayList<>();
        Mileagexy m=new Mileagexy();
        m.setAngle(30.331496472222224);
        m.setX(2864425.47798996);
        m.setY(37385681.63883557);
        m.setValue(5531.690890000001);
        m.setValueEnd(5581.690890000001);
        m.setType(2);
        m.setR(148.7839);
        m.setL(50.0);
        m.setSwing("左转");
        mileagexys.add(m);
        Mileagexy m1=new Mileagexy();
        m1.setAngle(31.698597055555556);
        m1.setX(2864282.57129165);
        m1.setY(37385595.72074135);
        m1.setValue(5364.940890000001);
        m1.setValueEnd(5531.690890000001);
        m1.setType(1);
        m1.setR(6988.565);
        m1.setL(166.75);
        m1.setSwing("左转");
        mileagexys.add(m1);
        Mileagexy m2=new Mileagexy();
        m2.setAngle(20.70414775);
        m2.setX(2864469.92360773);
        m2.setY(37385704.4054703);
        m2.setValue(5581.690890000001);
        m2.setValueEnd(5634.416790000001);
        m2.setType(1);
        m2.setR(148.7839);
        m2.setL(52.7259);
        m2.setSwing("左转");
        mileagexys.add(m2);
        String[] res = azimuthLC(2864441.350,37385690.766,90,mm,null,mileagexys,0);
//        mileagexy.setAngle(a - b03);
        System.out.println(res[0]);
        System.out.println(res[1]);
//        System.out.println(Math.sin(90* Math.PI/180));
//        System.out.println(Math.sin(0* Math.PI/180));
//       System.out.println(NumberUtil.sub(1.99951*100000, 1.99956*100000)/100000);
//        System.out.println(1.999519877503401*100000-1.999519877503401*100000);
//        System.out.println(NumberUtil.round(1.9995670643507062*100000, 5)-NumberUtil.round(1.999519877503401*100000, 4));
//        BigDecimal b1 = new BigDecimal(Double.toString(1.99971*100000));
//        BigDecimal b2 = new BigDecimal(Double.toString(1.99956*100000));
//        System.out.println( b1.subtract(b2).floatValue());

    }

}

