package com.imdroid.sideslope.bd;

import java.util.*;

/**
 * 1.过滤
 * 2.最多取最后300个点计算
 * 3.取最后100点求重心，画0.25cm的球
 * 4.求重心，包容百分之68，此重心为所求
 * 5.否则，扩大半径0.01再求重心，包容百分之68，此重心为所求
 * 6.。。。。。。
 */
public class FocusCalculator1 implements FocusCalculator{

    private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());
    private final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
    private Tilt tilt0;//初始状态
    private double[] position0;
    private double r = 10;//杆的长度cm 20230718 测试改为10
    //private boolean flag = false;  // 是否是第一次计算
    private final List<Point> pointList = new ArrayList<>();
    //public static double[] lastFocus = null;//
    private int delay_ms = 0;
    private int counter = 0;
    private boolean isShock = false;

    private final double shockThreshold = 1.5;

    private int counterNoB562 = 0;
    private int counterNoFixed = 0;
    private int counterFixedResult = 0;


    /**
     * 构建计算器，如果tilt0和position0是null，那么计算过程将不考虑与tilts数据融合
     * @param r 杆的长度，单位厘米
     * @param tilt0 一个小时前的tilt平均值，用一小时的avgTilt计算得到，如果第一次，用null
     * @param position0 一个小时前的xyz三维坐标值，用一小时前的ekf计算得到的那个，如果第一次，用null
     */
    public FocusCalculator1(double r, Tilt tilt0, double[] position0){
        this.r = r;
        this.tilt0 = tilt0;
        this.position0 = position0;
        this.isShock = false;
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
    }

    public FocusCalculator1(){

    }

    @Override
    public void reset(){
        list.clear();
        pointList.clear();
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
    }

    /**
     * 加入list
     * @param xyz
     */
    @Override
    public void addXyz(double[] xyz){
        if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
        else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++;
        else {
            counterFixedResult ++;
            if (filter(xyz)) {
                list.add(xyz);
            }
            if (list.size() > 300) {
                list.remove(0);
            }
        }
    }

    @Override
    public int[] getB562Stat(){
        return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
    }


    /**
     * 计算得到最终结果
     * @return
     */
    @Override
    public double[] resultB562(double[] r9250Result){
        int count = 50;
        double rate = 0.5;
        try {
            double r = 0.5;//初始球半径
            if(list.size() >= count){
                List<Point> results = new ArrayList<>();
                if (r9250Result != null && r9250Result.length > 0){
                    // 计算所有点位与变量之间的距离，并存入集合
                    pointList.clear();
                    for (double[] point : list) {
                        Point point1 = new Point(point[0],point[1],point[2]);
                        point1.setXDistance(disXY(point,r9250Result)); // 设置该点到变量的水平距离
                        point1.setZDistance(Math.abs(point[2] - r9250Result[2])); // 设置该点到变量的垂直距离
                        results.add(point1); // 将点位加入集合
                    }
                    Collections.sort(results, Comparator.comparing(Point::getXDistance));//排序

                    results = results.subList(0, Math.min(50, results.size()));

                }else {
                    for (int i = list.size()-count; i < list.size(); i++) {
                        results.add(new Point(list.get(i)[0],list.get(i)[1],list.get(i)[2]));
                    }
                    //flag = true;
                }

                // 求初始重心
                double[] focus = focusPoint(results);

                //重心
                int iterNum = 0;
                if(globeProportion(focus,r) < rate){
                    do{
                        iterNum++;
                        r += 0.1;
                        //球附近点集个数有可能为0，继续计算会导致focus结果出现NaN
                        List<double[]> doubles1 = globeFilter(focus, r);
                        if(doubles1.size() == 0){
                            continue;
                        }
                        focus = focus(doubles1);
                    }while (globeProportion(focus,r) < rate);
                    System.out.println("calc focus iter num:"+iterNum);
//                    if (flag){
//                    lastFocus = new double[3];
//                    lastFocus = focus;
//                    }
                    return focus;
                }else{
//                    if (flag){
//                        lastFocus = new double[3];
//                        lastFocus = focus;
//                    }
                    return focus;
                }

            }
        }catch (Exception e){
            //如果计算过程中修改list，可能引发异常
            e.printStackTrace();
        }
        return resultB562Sub();
    }

    /**
     * resultB562替补方法，尽量不返回空
     * @return x,y,z,有效点数
     */
    private double[] resultB562Sub(){
        if(list.size() > 0){
            //重心
            return focus(list);
        }
        return null;
    }

    /**
     * 过滤，剔除0,无穷大，与nan
     * @param xyz
     * @return true表示数据正常
     */
    private boolean filter(double[] xyz){
        double a = xyz[0]*xyz[1]*xyz[2];
        if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){
            return false;
        }
        return true;
    }

    /**
     * 三维点集重心计算
     * @param list
     * @return
     */
    private double[] focus(List<double[]> list){
        double sumX = 0;
        double sumY = 0;
        double sumZ = 0;
        int count = list.size();
        for (double[] doubles : list) {
            sumX += doubles[0];
            sumY += doubles[1];
            sumZ += doubles[2];
        }
        return new double[]{sumX/count,sumY/count,sumZ/count};
    }

    private double[] focusPoint(List<Point> list){
        double sumX = 0;
        double sumY = 0;
        double sumZ = 0;
        int count = list.size();
        for (Point point : list) {
            sumX += point.getX();
            sumY += point.getY();
            sumZ += point.getZ();
        }
        return new double[]{sumX/count,sumY/count,sumZ/count};
    }

    /**
     * 计算两个三维坐标的距离
     * @param a
     * @param b
     * @return
     */
    public static double dis(double[] a, double[] b){
        return Math.sqrt(Math.pow(a[0]-b[0],2) + Math.pow(a[1]-b[1],2) + Math.pow(a[2]-b[2],2));
    }

    public static double disXY(double[] a, double[] b){
        return Math.sqrt(Math.pow(a[0]-b[0],2) + Math.pow(a[1]-b[1],2));
    }

    /**
     * 重心附近点集占总数的比例
     * @param focus
     * @param r
     * @return
     */
    private double globeProportion(double[] focus, double r){
        int in = 0;//在球内的个数
        for (double[] doubles : list) {
            double dis = dis(doubles, focus);
            if(dis < r){
                in += 1;
            }
        }
        double proportion = 1.00*in/list.size();
        return proportion;
    }

    /**
     * 获取重心附近r内的所有点
     * @param focus
     * @param r
     * @return
     */
    private List<double[]> globeFilter(double[] focus, double r){
        ArrayList<double[]> arrayList = new ArrayList<>();
        for (double[] doubles1 : list) {
            if(dis(doubles1,focus) < r){
                arrayList.add(doubles1);
            }
        }
        return arrayList;
    }

    /**
     * tilt计算得到的坐标与b562数据得到的坐标融合
     * 1.可用点数大于50，优先相信b562
     * 2.可用点数大于0小于50，取b562与9250平均值
     * 3.可用点数无，用9250
     * @return
     */
    public double[] ekfResult(double[] b562Xyz, double[] tiltXyz){
        if(tiltXyz == null){
            return b562Xyz;
        }
        if(b562Xyz == null){
            return tiltXyz;
        }
        double b562Variance;
        double tiltVariance = 5; //Sherlock 230802 2-》5， 减小惯导的长周期影响 
        if(list.size() >= 50){
            b562Variance = 1;
        }else if(list.size() >= 40){
            b562Variance = 3;
        }else if(list.size() >= 30){
            b562Variance = 9;
        }else if(list.size() >= 20){
            b562Variance = 30;
        }else if(list.size() >= 10){
            b562Variance = 60;
        }else{
            b562Variance = 90;
        }
        double k = Math.pow(b562Variance,2)/(Math.pow(tiltVariance,2) + Math.pow(b562Variance,2));
        double x = b562Xyz[0] + k*(tiltXyz[0]-b562Xyz[0]);
        double y = b562Xyz[1] + k*(tiltXyz[1]-b562Xyz[1]);
        //z轴更加相信9250; 2023070改为5：1，20230706改为3：1， 20230718 改为 4：1，20230720 改为 5：1
        b562Variance = 5;
        tiltVariance = 1;
        k = Math.pow(b562Variance,2)/(Math.pow(tiltVariance,2) + Math.pow(b562Variance,2));
        double z = b562Xyz[2] + k*(tiltXyz[2]-b562Xyz[2]);
        return new double[]{x,y,z};
    }

    /**
     * 加入tilts
     * @param tilt
     */
    public void addTilt(Tilt tilt){
        tilts.add(tilt);
        if(tilt.getShock() > shockThreshold) isShock = true;

        if(tilts.size() > 300){
            tilts.remove(0);
        }
    }

    public boolean isShocked() {
        return isShock;
    }

    /**
     * 取平均
     * @return
     */
    public Tilt avgTilt(){
        if(tilts.size() == 0){
            return null;
        }
        try {
            Iterator<Tilt> iterator = tilts.iterator();
            int size = tilts.size();
            double sumP = 0;
            double sumR = 0;
            double sumY = 0;
            while (iterator.hasNext()){
                Tilt tilt = iterator.next();
                sumP += Math.sin(tilt.getPitch()*Math.PI/180);
                sumR += Math.sin(tilt.getRoll()*Math.PI/180);
                sumY += Math.sin(tilt.getYaw()*Math.PI/180);
            }
            double pitch = (Math.asin(sumP/size) * 180/Math.PI + 360)%360;
            double roll = (Math.asin(sumR/size) * 180/Math.PI + 360)%360;
            double yaw = (Math.asin(sumY/size) * 180/Math.PI + 360)%360;

            return new Tilt(pitch,roll,yaw);
        }catch (Exception e){
            e.printStackTrace();
        }
        return null;
    }

    public double[] result9250(){
        Tilt avg = avgTilt();
        if(tilt0 == null || position0 == null || r == 0 || avg == null){
            return null;
        }
        return TiltUtil.toPosition(avg,tilt0,position0,r);
    }

    public Tilt getTilt0(){
        return tilt0;
    }

    public double[] getPosition0(){
        return position0;
    }

    public double getR(){
        return r;
    }

    public void addDelayMs(int ms){
        delay_ms += ms;
        counter ++;
    }

    public int getAvgDelayMs(){
        return delay_ms/counter;
    }

}
