package com.imdroid.sideslope.bd;

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

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

/**
 * 1.如果本次有效解数量少于50个，本轮不处理，累计到下次
 * 2.enu分别排序，去掉前后20%，然后取交集，再求平均
 * 3.最多取最后500*6=3000个点计算
 */
public class FocusCalculator5 implements FocusCalculator{
    final Logger logger = LoggerFactory.getLogger(FocusCalculator5.class);
    // 固定解4元组：E、N、U、Quality
    final List<double[]> pointList = Collections.synchronizedList(new ArrayList<>());
    String deviceId;
    int minPointNum = 50;
    int maxPointNum = 3000;
    double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
    LocalDateTime referPointTime;
    short removeRate;

    // 惯导算法相关
    final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
    boolean isShock = false;
    final double shockThreshold = 1.5;

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

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

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

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

    public FocusCalculator5(){
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
        delay_ms = 0;
        delay_counter = 0;
        removeRate = 10;
    }

    public FocusCalculator5(String deviceId, int minPointNum, short removeRate){
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
        delay_ms = 0;
        delay_counter = 0;
        this.deviceId = deviceId;
        this.minPointNum = minPointNum;
        this.removeRate = (short) (removeRate/2);
    }

    public void setParas(String deviceId, int minPointNum, short removeRate){
        this.deviceId = deviceId;
        this.minPointNum = minPointNum;
        this.removeRate = (short) (removeRate/2);
    }

    @Override
    public void reset(){
        if(pointList.size()>=minPointNum) {
            pointList.clear();
        }
        else{
            logger.debug("{} num of fixed points:{}, less than {}",deviceId, pointList.size(),minPointNum);
        }
        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)) {//是否是无效值null
                pointList.add(xyz);
            }
            if (pointList.size() > maxPointNum) {
                pointList.remove(0);
            }
        }
    }

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

    @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", deviceId);
            referPoint = null;
        }
        return referPoint;
    }
    @Override
    public double[] resultB562(){
        if(pointList.size()<minPointNum) return null;
        //enu分别排序并取中间值
        List<double[]> eList = sortList(new EComparator());
        List<double[]> nList = sortList(new NComparator());
        List<double[]> uList = sortList(new UComparator());
        //取交集
        eList.retainAll(nList);
        eList.retainAll(uList);
        logger.debug("{} fixed point selected: {}", deviceId, eList.size());
        if(eList.size() == 0) return null;

        //求均值
        double eSum = 0;
        double nSum = 0;
        double uSum = 0;
        for(double[] item : eList){
            eSum += item[0];
            nSum += item[1];
            uSum += item[2];
        }
        return new double[]{eSum/eList.size(), nSum/eList.size(), uSum/eList.size()};
    }

    List<double[]> sortList(Comparator comparator){
        List<double[]> pList = new ArrayList<>();
        pList.addAll(pointList);
        pList.sort(comparator);
        int begin = pList.size()*removeRate/100;
        int end = pList.size()-begin;
        return pList.subList(begin, end);
    }


    /**
     * 过滤，剔除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;
    }

    /**
     * 加入tilts
     * @param tilt
     */
    @Override
    public void addTilt(Tilt tilt){
        tilts.add(tilt);
        if(tilt.getShock() > shockThreshold) isShock = true;

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

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

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

    @Override
    public boolean isJump() {
        return false;
    }

    @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 void addDelayMs(int ms){
        delay_ms += ms;
        delay_counter ++;
    }

    @Override
    public void addGGA(Gga gga) {

    }

    @Override
    public int getAvgDelayMs(){
        if(delay_counter==0) return -1;
        else return delay_ms/delay_counter;
    }

}
