import com.imdroid.sideslope.bd.FocusCalculator;
import com.imdroid.sideslope.bd.FocusCalculator1;
import com.imdroid.sideslope.bd.FocusCalculator2;
import com.imdroid.sideslope.bd.FocusCalculator5;
import org.ejml.simple.SimpleMatrix;
import org.junit.Test;
import org.junit.runner.RunWith;
import org.springframework.boot.test.context.SpringBootTest;
import org.springframework.test.context.junit4.SpringRunner;

import java.io.BufferedReader;
import java.io.FileReader;
import java.io.IOException;
import java.text.ParseException;
import java.text.SimpleDateFormat;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.Date;
import java.util.List;
import java.util.ListIterator;

@RunWith(SpringRunner.class)
@SpringBootTest(
        classes = {
                FocusCalculatorTest.class,
                FocusCalculator1.class,
                FocusCalculator2.class,
                FocusCalculator5.class
        }
)

public class FocusCalculatorTest {
    class CalcResult{
        double[] xyz;
        long time;
        boolean enabled = true;
    };

    final double xyTreshold = 3;
    final double zTreshold = 3;

    List<CalcResult> calcResultList = new ArrayList<>();
    List<CalcResult> filterResults = new ArrayList<>();

    FocusCalculator focusCalculator;
    String dataFileName = "src/test/java/data42.txt";

    @Test
    public void run() throws IOException, ParseException {
        System.out.println("*********** 2307042 *************");
        /*System.out.println("算法1");
        focusCalculator = new FocusCalculator1();
        test();
        System.out.println("算法2");
        focusCalculator = new FocusCalculator2();
        test();*/
        System.out.println("算法3");
        focusCalculator = new FocusCalculator5();
        ((FocusCalculator5)focusCalculator).setParas("2307042",50, (short) 20);
        test();
    }


    public void test() throws IOException, ParseException {
        FileReader fr = new FileReader(dataFileName);
        BufferedReader br = new BufferedReader(fr);
        String line = "";
        String[] arrs = null;
        SimpleDateFormat sdf = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss");

        long lastTime = 0;
        double[] last = null;

        while ((line = br.readLine()) != null) {
            arrs = line.split("\t");
            Date time = sdf.parse(arrs[0]+" "+arrs[1]);
            if(lastTime==0) lastTime = time.getTime();

            if(Math.abs(time.getTime() - lastTime)>30*60*1000) {
                last = calcGravity(last, lastTime);
                lastTime = time.getTime();
                //if(calcResultList.size()>=36) break;//18个小时
            }
            focusCalculator.addXyz(new double[]{
                    Double.parseDouble(arrs[2])*10,
                    Double.parseDouble(arrs[3])*10,
                    Double.parseDouble(arrs[4])*10,2},LocalDateTime.now());
        }
        br.close();
        fr.close();

        //if(focusCalculator.getB562Num()>0) {
            calcGravity(last, lastTime);
        //}

        System.out.println("raw results statistic:");
        calcStd(calcResultList);

        System.out.println("4 hours filter, average");
        runFilter(4);
        calcStd(filterResults);
        filterResults.removeAll(filterResults);
        /*
        System.out.println("4 hours filter, igg3");
        runFilter2(4);
        calcStd(filterResults);
        filterResults.removeAll(filterResults);

        System.out.println("8 hours filter, average");
        runFilter(8);
        calcStd(filterResults);
        filterResults.removeAll(filterResults);
        System.out.println("8 hours filter, igg3");
        runFilter2(8);
        calcStd(filterResults);
        filterResults.removeAll(filterResults);
*/
        calcResultList.removeAll(calcResultList);

    }

    double[] calcGravity(double[] last, long time){
        focusCalculator.setReferPoint(last);
        double[] result = focusCalculator.resultB562();
        // 解算结果处理
        CalcResult calcResult = new CalcResult();
        calcResult.xyz = result;
        calcResult.time = time;
        if(last!=null){
            //跳点
            if(Math.abs(result[0]-last[0])>xyTreshold ||
                    Math.abs(result[1]-last[1])>xyTreshold ||
                    Math.abs(result[2]-last[2])>zTreshold){
                calcResult.enabled = false;
            }
        }
        calcResultList.add(calcResult);
        focusCalculator.reset();
        return result;
    }

    void runFilter(int filterHour){
        long filterWin = filterHour*3600*1000;

        for(int i=0; i<calcResultList.size(); i++){
            CalcResult result = calcResultList.get(i);
            if(!result.enabled) continue;

            double[] filterXyz = new double[]{result.xyz[0],result.xyz[1],result.xyz[2]};
            long time = result.time;
            ListIterator listIterator = calcResultList.listIterator(i);
            int count = 1;
            while(listIterator.hasPrevious()){
                result = (CalcResult) listIterator.previous();
                if(result.enabled) {
                    if (time - result.time <= filterWin) {
                        filterXyz[0] += result.xyz[0];
                        filterXyz[1] += result.xyz[1];
                        filterXyz[2] += result.xyz[2];
                        count++;
                    } else break;
                }
            }
            filterXyz[0] /= count;
            filterXyz[1] /= count;
            filterXyz[2] /= count;
            CalcResult filterResult = new CalcResult();
            filterResult.xyz = filterXyz;
            filterResult.time = time;
            filterResults.add(filterResult);
        }
    }

    // IGG3
    void runFilter2(int filterHour){
        long filterWin = filterHour*3600*1000;
        List<Double> listE = new ArrayList<>();
        List<Double> listN = new ArrayList<>();
        List<Double> listD = new ArrayList<>();

        for(int i=0; i<calcResultList.size(); i++){
            CalcResult result = calcResultList.get(i);
            if(!result.enabled) continue;

            long time = result.time;
            double[] filterXyz = new double[]{result.xyz[0],result.xyz[1],result.xyz[2]};
            listE.add(result.xyz[0]);
            listN.add(result.xyz[1]);
            listD.add(result.xyz[2]);
            ListIterator listIterator = calcResultList.listIterator(i);
            while(listIterator.hasPrevious()){
                result = (CalcResult) listIterator.previous();
                if(result.enabled) {
                    if (time - result.time <= filterWin) {
                        listE.add(result.xyz[0]);
                        listN.add(result.xyz[1]);
                        listD.add(result.xyz[2]);
                    } else break;
                }
            }

            if(listE.size() > 1){
                double[] l = listE.stream().mapToDouble(Double::doubleValue).toArray();
                filterXyz[0] = igg3(l);
                l = listN.stream().mapToDouble(Double::doubleValue).toArray();
                filterXyz[1] = igg3(l);
                l = listD.stream().mapToDouble(Double::doubleValue).toArray();
                filterXyz[2] = igg3(l);
            }

            CalcResult filterResult = new CalcResult();
            filterResult.xyz = filterXyz;
            filterResult.time = time;
            filterResults.add(filterResult);
            listE.removeAll(listE);
            listN.removeAll(listN);
            listD.removeAll(listD);
        }
    }

    void calcStd(List<CalcResult> list){
        double[] first = list.get(0).xyz;
        double[] avg = {0,0,0};
        double[] max = {first[0],first[1],first[2]};
        double[] min = {first[0],first[1],first[2]};
        int count = 0;

        SimpleDateFormat format =  new SimpleDateFormat("yyyy-MM-dd HH:mm:ss"); //设置格式

        // 求均值
        for(CalcResult result:list){
            //System.out.println(format.format(result.time)+
            //        " e:"+result.xyz[0]+" n:"+result.xyz[1]+" d:"+result.xyz[2]+
            //        " valid:"+result.enabled);

            avg[0] += result.xyz[0];
            avg[1] += result.xyz[1];
            avg[2] += result.xyz[2];

            if(result.xyz[0]>max[0]) max[0] = result.xyz[0];
            if(result.xyz[1]>max[1]) max[1] = result.xyz[1];
            if(result.xyz[2]>max[2]) max[2] = result.xyz[2];

            if(result.xyz[0]<min[0]) min[0] = result.xyz[0];
            if(result.xyz[1]<min[1]) min[1] = result.xyz[1];
            if(result.xyz[2]<min[2]) min[2] = result.xyz[2];

            if(!result.enabled) count++;
        }
        avg[0] /= list.size();
        avg[1] /= list.size();
        avg[2] /= list.size();

        //求标准差
        double[] std = {0,0,0};
        for(CalcResult result:list){
            std[0] += Math.abs(avg[0] - result.xyz[0]);
            std[1] += Math.abs(avg[1] - result.xyz[1]);
            std[2] += Math.abs(avg[2] - result.xyz[2]);
        }
        std[0] /= list.size();
        std[1] /= list.size();
        std[2] /= list.size();
        System.out.println("total num: "+ list.size()+", bad num"+count);
        System.out.println("std: {"+ std[0]+", "+std[1]+", "+std[2]+"}");
        System.out.println("max delta: {"+ (max[0]-min[0])+", "+(max[1]-min[1])+", "+(max[2]-min[2])+"}");
    }

    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.05) {
                break;
            }
            x_prev = X.get(0);
            iterNum++;
        }

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

}