package com.imdroid.sideslope.bd;

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

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

/**
 * 1.取距离参考点最近的50%
 * 2.和参考点比较
 * 3.初始参考点更新时间和有效期
 * 4.压缩率
 */
public class FocusCalculator6 implements FocusCalculator{
    final Logger logger = LoggerFactory.getLogger(FocusCalculator6.class);
    // 固定解4元组：E、N、U、Quality
    final List<Point> pointList = Collections.synchronizedList(new ArrayList<>());
    String deviceId;
    int minPointNum = 50;
    int maxPointNum = 3000;
    double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
    LocalDateTime referPointExpiredTime;
    boolean isOrginalReferPointInUsed = false;

    double xyCompressRate = 0.2;
    double zCompressRate = 0.1;

    // 惯导算法相关
    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;

    public FocusCalculator6(){
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
        delay_ms = 0;
        delay_counter = 0;
    }

    public FocusCalculator6(String deviceId, int minPointNum){
        counterNoB562 = 0;
        counterNoFixed = 0;
        counterFixedResult = 0;
        delay_ms = 0;
        delay_counter = 0;
        this.deviceId = deviceId;
        this.minPointNum = minPointNum;
    }

    @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
                Point point = new Point(xyz[0], xyz[1], xyz[2]);
                if(referPoint!=null){
                    point.setXDistance(Math.abs(xyz[0]-referPoint[0]));
                    point.setYDistance(Math.abs(xyz[1]-referPoint[1]));
                    point.setZDistance(Math.abs(xyz[2]-referPoint[2]));
                }
                pointList.add(point);
            }
            if (pointList.size() > maxPointNum) {
                pointList.remove(0);
            }
        }
    }

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

    @Override
    public void setReferPoint(double[] point){
        LocalDateTime now = LocalDateTime.now();
        // 如果初值设置在24小时内，仍使用初值作为参考值
        if(isOrginalReferPointInUsed){
            if(now.isBefore(referPointExpiredTime)) return;
            else {
                logger.info("{} original refer point expired, changed to new point", deviceId);
                isOrginalReferPointInUsed = false;
            }
        }
        referPoint = point;
        referPointExpiredTime = now.plusHours(24);
    }

    public void setOriginalReferPoint(double[] point){
        logger.info("{} set original refer point {},{},{}", deviceId, point[0],point[1],point[2]);

        isOrginalReferPointInUsed = true;
        referPoint = point;
        referPointExpiredTime = LocalDateTime.now().plusHours(24);
    }

    @Override
    public double[] getReferPoint(){
        if(referPoint==null || referPointExpiredTime==null) {
            return null;
        }

        if(LocalDateTime.now().isAfter(referPointExpiredTime)) {
            logger.info("{} refer time is expired", deviceId);
            referPoint = null;
        }
        return referPoint;
    }

    @Override
    public double[] resultB562(){
        if(pointList.size()<minPointNum) return null;

        double focusX = sortAndAvg(Point::getXDistance, Point::getX, referPoint != null);
        double focusY = sortAndAvg(Point::getYDistance, Point::getY, referPoint != null);
        double focusZ = sortAndAvg(Point::getZDistance, Point::getZ, referPoint != null);

        if(referPoint!=null){
            focusX = referPoint[0] + (focusX - referPoint[0])*xyCompressRate;
            focusY = referPoint[1] + (focusY - referPoint[1])*xyCompressRate;
            focusZ = referPoint[2] + (focusZ - referPoint[2])*zCompressRate;
        }
        return new double[]{focusX, focusY, focusZ};
    }

    double sortAndAvg(Function<Point, Double> sortFunc, Function<Point, Double> avgFunc, boolean hasRef){
        List<Point> points;
        if(hasRef) {
            pointList.sort(Comparator.comparing(sortFunc));
            points = pointList.subList(0, pointList.size() / 2);
        }
        else{
            pointList.sort(Comparator.comparing(avgFunc));
            int begin = pointList.size()/4;
            points = pointList.subList(begin, pointList.size()-begin);
        }
        return avgPoint(points, avgFunc);
    }

    double avgPoint(List<Point> points, Function<Point, Double> getFunction){
        if(points.size() == 0) return 0;
        double sumValue = 0;
        for (Point point:points){
            sumValue += getFunction.apply(point);
        }
        return sumValue/points.size();
    }

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

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

}
