package com.imdroid.sideslope.bd;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.time.LocalDateTime;
import java.util.*;

/**
 * 1.过滤
 * 2.最多取最后300个点计算
 * 3.取最后100点求重心，画0.25cm的球
 * 4.求重心，包容百分之68，此重心为所求
 * 5.否则，扩大半径0.01再求重心，包容百分之68，此重心为所求
 * 6.。。。。。。
 */
public class FocusCalculator1 implements FocusCalculator{
    final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class);
    // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数
    final List<double[]> pointList = Collections.synchronizedList(new ArrayList<>());
    int gravityMinCount = 50;
    int gravityMaxCount = 300;
    double gravityInitR = 5;//mm
    double gravityMaxR = 300;//mm
    double pointSelectedRate = 0.5;
    double iterStep = 1;//mm
    double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
    LocalDateTime referPointTime;

    // 惯导算法相关
    final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
    //Tilt tilt0;//初始状态
    //double[] position0;
    //double height = 2000;
    boolean isShock = false;
    final double shockThreshold = 1.5;

    //其他
    int delay_ms;
    int delay_counter;
    int counterNoB562;
    int counterNoFixed;
    int counterFixedResult;

    /*public FocusCalculator1(double height, Tilt tilt0, double[] position0){
        this.height = height;
        this.tilt0 = tilt0;
        this.position0 = position0;
        this.isShock = false;
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
    }*/

    public FocusCalculator1(){
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
        delay_ms = 0;
        delay_counter = 0;
    }

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

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

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


    /**
     * 计算本轮固定解的重心：
     * 1、如果本轮固定解数少于50个，则直接计算均值
     * 2、如果大于50个：
     * a）找出离上次距离最近的50个点，计算初始重心
     * b）以初始重心为球心，初始半径为0.5画球，找出所有在球内的点
     * c）重新计算这个球的重心，半径增加0.1，再次找出所有在球内的点，直到筛选出不少于总数50%的点
     * d）计算这些点的均值
     */
    @Override
    public void setReferPoint(double[] point){
        referPoint = point;
        referPointTime = LocalDateTime.now();
    }
    @Override
    public double[] getReferPoint(){
        if(referPoint==null || referPointTime==null) {
            return null;
        }

        if(LocalDateTime.now().isAfter(referPointTime.plusHours(24))) {
            logger.info("refer time is expired");
            referPoint = null;
        }
        return referPoint;
    }
    @Override
    public double[] resultB562(){
        double r = gravityInitR;//初始球半径
        try {
            if(pointList.size() >= gravityMinCount){
                // 筛选出离上次解算结果最近的50个点计算初始球心
                // 如果没有上次位置，选最后50个点
                // 为便于排序，使用Point对象
                List<Point> selectPoints = new ArrayList<>();
                if (referPoint != null && referPoint.length > 0){
                    // 计算所有点位与变量之间的距离，并存入集合
                    for (double[] point : pointList) {
                        Point pointObj = new Point(point[0],point[1],point[2]);
                        pointObj.setXDistance(disXY(point,referPoint)); // 设置该点到变量的水平距离
                        pointObj.setZDistance(Math.abs(point[2] - referPoint[2])); // 设置该点到变量的垂直距离
                        selectPoints.add(pointObj); // 将点位加入集合
                    }
                    Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序

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

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

                // 求初始重心
                double[] focus = focusPointObj(selectPoints);
                logger.info("seed focus {},{},{},select size {}",focus[0],focus[1],focus[2],selectPoints.size());

                // 迭代计算重心，直到选出50%的点
                List<double[]> filteredPoints=null;
                if(globeProportion(focus,r) < pointSelectedRate){
                    do{
                        r += iterStep;
                        if(r>=gravityMaxR) break;
                        //球附近点集个数有可能为0，继续计算会导致focus结果出现NaN
                        filteredPoints = globeFilter(focus, r);
                        if(filteredPoints.size() == 0) continue;
                        focus = focusPoint(filteredPoints);
                    }while ((double)filteredPoints.size()/pointList.size() < pointSelectedRate);
                    if(null!=filteredPoints) logger.info("calc focus r:{}, point num:{}",r,filteredPoints.size());
                    return focus;
                }else{
                    logger.info("calc focus iter num:0, point num:{}",selectPoints.size());
                    return focus;
                }

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

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

    /**
     * 过滤，剔除0,无穷大，与nan
     * @param xyz
     * @return true表示数据正常
     */
    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
     */
    double[] focusPoint(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};
    }

    double[] focusPointObj(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
     */
    double globeProportion(double[] focus, double r){
        int in = 0;//在球内的个数
        for (double[] doubles : pointList) {
            double dis = dis(doubles, focus);
            if(dis < r){
                in += 1;
            }
        }

        return 1.00*in/pointList.size();
    }

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

    /**
     * tilt计算得到的坐标与b562数据得到的坐标融合
     * 1.可用点数大于50，优先相信b562
     * 2.可用点数大于0小于50，取b562与9250平均值
     * 3.可用点数无，用9250
     * @return
     */
    /*@Override
    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(pointList.size() >= 50){
            b562Variance = 1;
        }else if(pointList.size() >= 40){
            b562Variance = 3;
        }else if(pointList.size() >= 30){
            b562Variance = 9;
        }else if(pointList.size() >= 20){
            b562Variance = 30;
        }else if(pointList.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
     */
    @Override
    public void addTilt(Tilt tilt){
        tilts.add(tilt);
        if(tilt.getShock() > shockThreshold) isShock = true;

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

    @Override
    public boolean isShocked() {
        return isShock;
    }

    @Override
    public int getVer() {
        return 1;
    }

    @Override
    public Tilt avgTilt() {
        if (tilts.size() == 0) {
            return null;
        }
        Iterator<Tilt> iterator = tilts.iterator();
        double roll = 0;
        double pitch = 0;
        double yaw = 0;
        while (iterator.hasNext()) {
            Tilt tilt = iterator.next();
            roll += tilt.getRoll();
            pitch += tilt.getPitch();
            yaw += tilt.getYaw();
        }
        return new Tilt(pitch/tilts.size(),roll/tilts.size(),yaw/tilts.size());
    }
/*
    @Override
    public double[] result9250(){
        Tilt avg = avgTilt();
        if(tilt0 == null || position0 == null || height == 0 || avg == null){
            return null;
        }
        return TiltUtil.toPosition(avg,tilt0,position0,height);
    }
*/
    @Override
    public void addDelayMs(int ms){
        delay_ms += ms;
        delay_counter ++;
    }
    @Override
    public int getAvgDelayMs(){
        if(delay_counter==0) return -1;
        else return delay_ms/delay_counter;
    }

}
