package com.imdroid.sideslope.bd;

import java.util.*;

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

    private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());

    private final List<Point> pointList = new ArrayList<>();
    private int counterNoB562 = 0;
    private int counterNoFixed = 0;
    private int counterFixedResult = 0;


    @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);
            }
        }
    }

    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;
    }

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


    /**
     * 计算得到最终结果
     * @return
     */
    public double[] resultB562_2() {
        // 求全体样本的重心
        double e = 0;
        double n = 0;
        double d = 0;
        for(double[] point: list){
            e += point[0];
            n += point[1];
            d += point[2];
        }
        e /= list.size();
        n /= list.size();
        d /= list.size();

        double g = 0;
        int iterNum = 0;
        while(Math.abs(g-e)>0.2){
            g = e;
            e = gravity(g, 0.3, 0);
            iterNum ++;
            if(iterNum>=10) break;
        }
        //System.out.println("e iter num: "+iterNum);

        g = 0;
        iterNum = 0;
        while(Math.abs(g-n)>0.2){
            g = n;
            n = gravity(g, 0.26, 0);
            iterNum ++;
            if(iterNum>=10) break;
        }
        //System.out.println("n iter num: "+iterNum);

        g = 0;
        iterNum = 0;
        while(Math.abs(g-d)>0.4){
            g = d;
            d = gravity(g, 0.87, 0);
            iterNum ++;
            if(iterNum>=10) break;
        }
        //System.out.println("e iter num: "+iterNum);

        return new double[]{e,n,d};
    }

    double gravity(double g, double r, int index){
        //筛选出半径为r=0.144的圆内的点
        double newG = 0;
        List<Double> list2 = new ArrayList<>();
        for(double[] point: list){
            if(Math.abs(point[index]-g)<=r){
                list2.add(point[index]);
                newG += point[index];
            }
        }
        System.out.println("sub set: " + list2.size());
        if(list2.size()>0) newG /= list2.size();
        return newG;
    }

    @Override
    public double[] resultB562(double[] last) {
        double[] g = {0, 0, 0};
        if(last != null) g=last;
        else {
            for (double[] xyz : list) {
                g[0] += xyz[0];
                g[1] += xyz[1];
                g[2] += xyz[2];
            }
            g[0] /= list.size();
            g[1] /= list.size();
            g[2] /= list.size();
        }

        double[] g2 = {0,0,0};
        int iterNum = 0;
        while(isLarge(g,g2,2, 4)){
            g2 = g;
            g = gravity3(g);
            iterNum ++;
            if(iterNum>=10) break;
        }
        System.out.println("e iter num: "+iterNum);

        return g;
    }

    boolean isLarge(double[] p1, double[] p2, double xy, double z){
        //return true;
        return (Math.sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1]))>xy ||
                Math.abs(p1[2]-p2[2])>z);
    }

    double[] gravity2(double[] g){
        //筛选出半径为r=0.144的圆内的点
        List<Point> points = new ArrayList<>();
        for(double[] xyz: list){
            Point point = new Point(xyz[0], xyz[1], xyz[2]);
            point.setXDistance(Math.sqrt((xyz[0]-g[0])*(xyz[0]-g[0])+(xyz[1]-g[1])*(xyz[1]-g[1])));
            point.setZDistance(Math.abs(xyz[2]-g[2]));
            points.add(point);
        }

        double[] newG = {0,0,0};
        int i=0;
        int calcNum = points.size()*5/10;
        Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序
        for(Point point:points){
            newG[0] += point.getX();
            newG[1] += point.getY();
            i++;
            if(i>=calcNum) {
                newG[0] /= calcNum;
                newG[1] /= calcNum;
                break;
            }
        }

        Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序
        i=0;
        for(Point point:points){
            newG[2] += point.getZ();
            i++;
            if(i>=calcNum) {
                newG[2] /= calcNum;
                break;
            }
        }

        return newG;
    }

    double[] gravity3(double[] g){
        //筛选出半径为r=0.144的圆内的点
        List<Point> points = new ArrayList<>();
        for(double[] xyz: list){
            Point point = new Point(xyz[0], xyz[1], xyz[2]);
            point.setXDistance(Math.abs(xyz[0]-g[0]));
            point.setYDistance(Math.abs(xyz[1]-g[1]));
            point.setZDistance(Math.abs(xyz[2]-g[2]));
            points.add(point);
        }

        double[] newG = {0,0,0};
        int i=0;
        int calcNum = points.size()*5/10;
        Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序
        for(Point point:points){
            newG[0] += point.getX();
            i++;
            if(i>=calcNum) {
                newG[0] /= calcNum;
                break;
            }
        }

        Collections.sort(points, Comparator.comparing(Point::getYDistance));//升序排序
        i=0;
        for(Point point:points){
            newG[1] += point.getY();
            i++;
            if(i>=calcNum) {
                newG[1] /= calcNum;
                break;
            }
        }

        Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序
        i=0;
        for(Point point:points){
            newG[2] += point.getZ();
            i++;
            if(i>=calcNum) {
                newG[2] /= calcNum;
                break;
            }
        }

        return newG;
    }
}
