package com.imdroid.sideslope.bd;

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

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

    final List<Map.Entry<LocalDateTime, List<double[]>>> cyclePointList = Collections.synchronizedList(new ArrayList<>());

    final static int INIT_REFER_POINT_NOT_SET = 0;
    final static int INIT_REFER_POINT_IN_USE = 1;
    final static int INIT_REFER_POINT_EXPIRED = 2;
    int originalReferPointFlag= INIT_REFER_POINT_NOT_SET; // 0:not set; 1:in use; 2:expired
    LocalDateTime originalReferPointTime;

    class EComparator implements Comparator<double[]>{
         @Override
         public int compare(double[] point1, double[] point2) {
             return (int) ((point1[0] - point2[0])*100);
         }
     }

    class NComparator implements Comparator<double[]>{
        @Override
        public int compare(double[] point1, double[] point2) {
            return (int) ((point1[1] - point2[1])*100);
        }
    }

    class DComparator implements Comparator<double[]>{
        @Override
        public int compare(double[] point1, double[] point2) {
            return (int) ((point1[2] - point2[2])*100);
        }
    }

    /**
     * 加入pointList
     * @param xyz
     */
    @Override
    public void addXyz(double[] xyz, LocalDateTime dataTime){
        Map.Entry entry = null;
        if(cyclePointList.size()==0 || counterFixedResult+counterNoB562+counterNoFixed == 0){
            entry = new AbstractMap.SimpleEntry(dataTime, new ArrayList<double[]>());
            cyclePointList.add(entry);
            // 检查有没有过期的数据
            Iterator<Map.Entry<LocalDateTime, List<double[]>>> iterator = cyclePointList.iterator();
            while (iterator.hasNext()){
                LocalDateTime t = iterator.next().getKey();
                if(t.isBefore(dataTime.minusHours(B562_EXPIRED_HOUR))){
                    iterator.remove();
                }
            }
        }
        else{
            entry = cyclePointList.get(cyclePointList.size()-1);
        }

        if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
        else if((int)xyz[3] == UBXUtil.FLOAT_RESULT) counterNoFixed++;
        else {
            counterFixedResult ++;
            if (filter(xyz)) {
                List<double[]> list = (List<double[]>) entry.getValue();
                list.add(xyz);
            }
        }
    }

    double calcGravity(List<double[]> list , int index){
        double sum = 0;
        int begin = (int) (list.size() * 0.25);
        int end = (int) (list.size() * 0.75);
        if(end-begin == 0) return 0;

        for(int i=begin; i<end; i++){
            sum += list.get(i)[index];
        }
        return sum/(end-begin);
    }

    public double[] resultB562(){
        // 把本轮的固定解添加到pointList一起解算
        if(counterFixedResult<30) return null;
        // 求pointList的重心：当固定解大于300个时，去掉25%最大值和25%最小值
        pointList.clear();
        Iterator<Map.Entry<LocalDateTime, List<double[]>>> iterator = cyclePointList.iterator();
        while (iterator.hasNext()){
            pointList.addAll(iterator.next().getValue());
        }
        logger.info("total fixed point num to calculated: {},{},{}",pointList.size(),
                cyclePointList.get(0).getKey(), cyclePointList.get(cyclePointList.size()-1).getKey());

        if(pointList.size() >= gravityMaxCount){
            Collections.sort(pointList, new EComparator());
            double e = calcGravity(pointList,0);
            Collections.sort(pointList, new NComparator());
            double n = calcGravity(pointList,1);
            Collections.sort(pointList, new DComparator());
            double d = calcGravity(pointList,2);
            return new double[]{e,n,d};
        }

        return null;
    }

    @Override
    public boolean isShocked() {
        //if(tilt0 == null) return false;
        Tilt avg = avgTilt();
        if(avg == null) return false;

        isShock = (Math.abs(avg.getPitch())+Math.abs(avg.getRoll()) > 10);
        return isShock;
    }

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

    @Override
    public void setReferPoint(double[] point){
        if(originalReferPointFlag==INIT_REFER_POINT_IN_USE){
            // 初始参考位置保持24小时
            if(originalReferPointTime!=null &&
                    LocalDateTime.now().isBefore(originalReferPointTime.plusHours(24))){
                return;
            }
            else originalReferPointFlag = INIT_REFER_POINT_EXPIRED;
        }
        referPoint = point;
        referPointTime = LocalDateTime.now();
    }

    public boolean isOriginalReferPointSet(){
        return (originalReferPointFlag!=INIT_REFER_POINT_NOT_SET);
    }
    public void setOriginalReferPoint(double[] point){
        referPoint = point;
        referPointTime = LocalDateTime.now();
        originalReferPointFlag = INIT_REFER_POINT_IN_USE;
        originalReferPointTime = LocalDateTime.now();
    }
}
