import com.imdroid.sideslope.bd.FocusCalculator;
import com.imdroid.sideslope.bd.Point;
import org.ejml.simple.SimpleMatrix;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

public class FocusCalculator3 implements FocusCalculator {

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

    private final List<Double> listE = Collections.synchronizedList(new ArrayList<>());

    private final List<Double> listN = Collections.synchronizedList(new ArrayList<>());

    private final List<Double> listD = Collections.synchronizedList(new ArrayList<>());

    private final List<Point> pointList = new ArrayList<>();


    @Override
    public void reset(){
        list.removeAll(list);
        pointList.removeAll(pointList);
        listE.removeAll(listE);
        listD.removeAll(listD);
        listN.removeAll(listN);
    }

    /**
     * 加入list
     * @param xyz
     */
    @Override
    public void addXyz(double[] xyz){
        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[]{0,0,0};
    }

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

        makeMatrixList(g);

        double[] l = listE.stream().mapToDouble(Double::doubleValue).toArray();
        double e = igg3(l);
        l = listN.stream().mapToDouble(Double::doubleValue).toArray();
        double n = igg3(l);
        l = listD.stream().mapToDouble(Double::doubleValue).toArray();
        double d = igg3(l);

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

    void makeMatrixList(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);
        }

        int calcNum = points.size()*5/10;
        Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序
        for(Point point:points){
            listE.add(point.getX());
            if(listE.size()>=calcNum) {
                break;
            }
        }

        Collections.sort(points, Comparator.comparing(Point::getYDistance));//升序排序
        for(Point point:points){
            listN.add(point.getY());
            if(listN.size()>=calcNum) {
                break;
            }
        }

        Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序
        for(Point point:points){
            listD.add(point.getZ());
            if(listD.size()>=calcNum) {
                break;
            }
        }
    }

    public static double igg3(double[] l)  {
        SimpleMatrix L = new SimpleMatrix(l.length,1,false,l);
        SimpleMatrix B = new SimpleMatrix(l.length,1);
        B.fill(1);
        SimpleMatrix P = new SimpleMatrix(l.length, l.length);
        for(int i=0; i<l.length;i++){
            //P.set(i,i,1/l[i]);
            P.set(i,i,1);
        }
        double x_prev=0;
        double x0=0;
        for(int i=0;i<l.length;i++){
            x0 += l[i];
        }
        x0 /= l.length;

        L = L.minus(x0);
        SimpleMatrix W,N,X,V,Q,Qvv;
        SimpleMatrix sigma0;
        double s0;
        double k0=1.0, k1=5, k=1.0;
        int iterNum = 0;

        while(iterNum<10){
            W = B.transpose().mult(P).mult(L);  //W=B'*P*l;
            N = B.transpose().mult(P).mult(B);  //N=B'*P*B;
            X = N.invert().mult(W);             //x=inv(N)*W;% 待估参数向量
            V = B.mult(X).minus(L);             //v=B*x-l;% 残差向量

            sigma0 = V.transpose().mult(P).mult(V).divide((l.length-1));
            s0 = Math.sqrt(sigma0.get(0,0));//sigma0=sqrt(v'*P*v/(10-1));% 单位权中误差
            //System.out.println(X);
            //System.out.println(s0);
            Q = P.invert();//Q=inv(P);
            Qvv = Q.minus(B.mult(N.invert().mult(B.transpose())));//Qvv=Q-B*inv(N)*B';

            for(int i=0; i<l.length; i++){
                double v = V.get(i)/(s0*Math.sqrt(Qvv.get(i,i))); //v_=v(i)/(sigma0*sqrt(Qvv(i,i)));
                if(Math.abs(v) < k0){
                    k = 1.0;
                }
                else if(Math.abs(v) > k1){
                    k = 1e-8;
                }
                else{
                    k=(k1-Math.abs(v))/(k1-k0);
                    k=k*k*(k0/Math.abs(v));
                }
                P.set(i,i,P.get(i,i)*k); //P(i,i)=P(i,i)*k;
            }

            if(x_prev==0) {
                x_prev = X.get(0);
            }
            else if(Math.abs(x_prev-X.get(0))<0.1) {
                break;
            }
            x_prev = X.get(0);
            iterNum++;
        }

        //x=x0+x_prev;%求出的最后长度 初值+改正数
        return (x0+x_prev);
    }

}
