package com.imdroid.sideslope.bd;

import com.imdroid.sideslope.service.Device;

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

import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.*;

/**
 * 博通：用GGA绝对坐标代替相对位置
 */
public class FocusCalculator3 extends FocusCalculator1{
    //final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
    final static int bad_change_mm = 800;//固定解水平跳变连续10次超过800mm，认为是周跳
    final static int bad_duration = 10;

    int bad_count = 0;
    Device bsDevice;

    public FocusCalculator3(Device bsDevice){
        super();
        this.bsDevice = bsDevice;
        gravityMinCount = 80;
    }

    @Override
    public void addGGA(Gga gga) {
        if(gga == null) return;
        double[] end;

        // 测站的GGA - 测站 LLA 数据
        LLA_Coordinate rover_lla = new LLA_Coordinate(gga.getLatitude(),gga.getLongitude(),gga.getAltitude());
        // 测站 LLA 坐标转 ECEF 坐标，单位米
        ECEF_Coordinate rover_ecef = LLA2ECEF(rover_lla);

        if(bsDevice==null || bsDevice.getEcefx()==null){
            end = new double[]{
                    rover_ecef.getECEF_X()*1000,
                    rover_ecef.getECEF_Y()*1000,
                    rover_ecef.getECEF_Z()*1000,
                    gga.getQuality()};
        }
        else {
            // 查询测站的绑定的基站
            ECEF_Coordinate reference_ecef = new ECEF_Coordinate(bsDevice.getEcefx(), bsDevice.getEcefy(), bsDevice.getEcefz());

            // 以基站为站心的 ENU 坐标
            ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef);

            end = new double[]{
                    difference_enu.ENU_E*1000,
                    difference_enu.ENU_N*1000,
                    difference_enu.ENU_U*1000,
                    gga.getQuality()};
        }

        if(gga.isFixed()) {
            counterFixedResult++;
            if(pointList.size()>0){
                double[] lastXyz = pointList.get(pointList.size()-1);
                if((Math.abs(end[0]-lastXyz[0])+Math.abs(end[1]-lastXyz[1])>bad_change_mm )
                        || (Math.abs(end[2]-lastXyz[2])>bad_change_mm)){
                    bad_count++;
                    return;
                }
            }
            bad_count = 0;

            pointList.add(end);
        }
        else if(gga.getQuality() == 5) counterNoFixed++;
        else counterNoB562 ++;
    }

    /**
     * 加入pointList
     * @param xyz
     */
    @Override
    public void addXyz(double[] xyz, LocalDateTime dataTime){

    }

    @Override
    public double[] resultB562(){
        try {
            if (pointList.size() >= gravityMinCount) {
                List<Point> selectPoints = new ArrayList<>();
                if (referPoint != null && referPoint.length > 0) {
                    // 计算所有点位与变量之间的距离，并存入集合
                    for (double[] point : pointList) {
                        Point pointObj = new Point(point[0], point[1], point[2]);
                        pointObj.setXDistance(Math.abs(point[0] - referPoint[0])); // 设置该点到变量的水平距离
                        pointObj.setYDistance(Math.abs(point[1] - referPoint[1])); // 设置该点到变量的水平距离
                        pointObj.setZDistance(Math.abs(point[2] - referPoint[2])); // 设置该点到变量的垂直距离
                        selectPoints.add(pointObj); // 将点位加入集合
                    }
                    //分别对东北天排序
                    Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序
                    List<Point> points = selectPoints.subList(0, selectPoints.size() / 2);
                    double[] focusX = focusPointObj(points);
                    Collections.sort(selectPoints, Comparator.comparing(Point::getYDistance));//排序
                    points = selectPoints.subList(0, selectPoints.size() / 2);
                    double[] focusY = focusPointObj(points);
                    Collections.sort(selectPoints, Comparator.comparing(Point::getZDistance));//排序
                    points = selectPoints.subList(0, selectPoints.size() / 2);
                    double[] focusZ = focusPointObj(points);
                    return new double[]{focusX[0], focusY[1], focusZ[2]};

                }
                else {
                    for (int i = pointList.size()/2; i < pointList.size(); i++) {
                        selectPoints.add(new Point(pointList.get(i)[0], pointList.get(i)[1], pointList.get(i)[2]));
                    }
                    // 求重心
                    double[] focus = focusPointObj(selectPoints);
                    return focus;
                }
            }
        }
        catch (Exception e){
            logger.error("error!",e);
            return null;
        }
        return null;
    }

    @Override
    public void reset(){
        super.reset();
        bad_count = 0;
    }

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

    public boolean isJump(){
        if (bad_count>bad_duration){
            bad_count = 0;
            return true;
        }
        else return false;
    }

}
