package com.imdroid.sideslope.calc;

import com.imdroid.secapi.dto.GnssCalcData;
import com.imdroid.sideslope.bd.*;
import com.imdroid.sideslope.message.D341LocationMessage;
import com.imdroid.sideslope.service.WarningService;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import java.time.LocalDateTime;
import java.util.Arrays;
import java.util.Map;
import java.util.concurrent.*;
/**
 * @author Layton
 * @date 2023/2/4 19:22
 */
@Service
public class SingleLineGNSSCalcService implements GNSSDataCalcService {

    private static final Logger logger = LoggerFactory.getLogger(SingleLineGNSSCalcService.class);

    private static final Map<String, FocusCalculator1> calculatorMap = new ConcurrentHashMap<>();

    private static final Map<String, ScheduledFuture<?>> timerMap = new ConcurrentHashMap<>();

    private static final Map<String, Tilt> tiltMap = new ConcurrentHashMap<>();

    private static final Map<String, double[]> positionMap = new ConcurrentHashMap<>();

    private static final Map<String, Boolean> cleanTiltStatusMap = new ConcurrentHashMap<>();

    private static final Map<String, Boolean> cleanPositionStatusMap = new ConcurrentHashMap<>();

    private static final ScheduledExecutorService executorService = Executors.newScheduledThreadPool(4);

    @Autowired
    WarningService warningService;

    @Autowired
    GNSSCalcFilterService gnssCalcFilterService;

    @Override
    public double[] calcSingle(D341LocationMessage message, boolean completeWhenIdle) {
        String deviceId = message.getId();
        if(completeWhenIdle) resultOutputTimer(deviceId, message.getTenantId(), message.getCreateTime());

        //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
        FocusCalculator1 focusCalculator = calculatorMap.computeIfAbsent(deviceId, s -> new FocusCalculator1(150,tiltMap.get(deviceId),
                positionMap.get(deviceId)));
        // 读取惯导
        Tilt tilt = message.getTilt();

        focusCalculator.addTilt(tilt);
        if (logger.isDebugEnabled()) {
            logger.debug("测站" + deviceId + "的9250单次解析结果:{}", tilt + "," + Arrays.toString(TiltUtil.toPosition(tilt, focusCalculator.getTilt0(), focusCalculator.getPosition0(), focusCalculator.getR())));
        }

        // 计算延迟
        focusCalculator.addDelayMs(message.getPps());

        //计算到单次相对位置xyz并记录
        double[] doubles = message.getB562_loc();
        focusCalculator.addXyz(doubles);
        if (logger.isDebugEnabled()) {
            logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
        }
        return doubles;
    }

    @Override
    public void calSingleDone(String deviceId, Integer tenantId, LocalDateTime resultTime) {
        executorService.schedule(() -> {
            try {
                calCycleResult(deviceId, tenantId, resultTime);
            } catch (Exception e) {
                logger.error(e.toString());
            }
        },50, TimeUnit.MILLISECONDS);

    }

    private void resultOutputTimer(String deviceId, Integer tenantId, LocalDateTime date){
        //40秒没数据后输出结果
        ScheduledFuture<?> future = timerMap.get(deviceId);
        if (future != null && !future.isDone()) {
            future.cancel(true);
            future = null;
        }

        future = executorService.schedule(() -> {
            try {
                calCycleResult(deviceId, tenantId, date);
            } catch (Exception e) {
                logger.error(e.toString());
            }
        },40, TimeUnit.SECONDS);
        timerMap.put(deviceId, future);
    }

    private void calCycleResult(String deviceId, Integer tenantId, LocalDateTime resultTime) {
        FocusCalculator1 focusCalculator = calculatorMap.get(deviceId);
        if(focusCalculator == null) return;
        // 1.检查b562有效数，如果过少，产生告警
        warningService.checkB562Num(deviceId, tenantId,
                focusCalculator.getB562Stat());

        // 2.数据处理：参考上次的位置，计算b562重心
        double[] lastEfk = null;
        if(positionMap.containsKey(deviceId)){
            lastEfk = positionMap.get(deviceId);
        }
        double[] b562Result = focusCalculator.resultB562(lastEfk);
        double[] r9250Result = null;
        //判断 取到的b562  和上次融合坐标做判断，如果距离>100mm  不计算9250
        if (lastEfk != null && FocusCalculator1.disXY(b562Result,lastEfk)<100){
            r9250Result = focusCalculator.result9250();
        }
        double[] result = focusCalculator.ekfResult(b562Result,r9250Result);
        Tilt tilt = calcAvgTilt(deviceId);

        logger.info("测站 {} 的b562相对坐标重心:{}", deviceId, Arrays.toString(b562Result));
        logger.info("测站 {} 的9250相对坐标:{}", deviceId, Arrays.toString(r9250Result));
        logger.info("测站 {} 的相对坐标融合值:{}", deviceId, Arrays.toString(result));
        logger.info("测站 {} 的Tilt平均值:{}", deviceId, tilt);
        logger.info("测站 {} 的平均延迟:{}ms", deviceId, focusCalculator.getAvgDelayMs());

        if (tilt != null) {
            if (Boolean.TRUE.equals(cleanTiltStatusMap.get(deviceId))) {
                // 置为false，下一次计算时就可以正常设置惯导值到tiltMap了
                cleanTiltStatusMap.put(deviceId, Boolean.FALSE);
                logger.info("置为false，下一次计算时就可以正常设置惯导值到tiltMap了");
            } else {
                tiltMap.put(deviceId, tilt);
            }
        }
        if (result != null && b562Result != null) {
            //这里检查一下result，过滤1千米外非正常数据
            if(focusCalculator.dis(result,b562Result) < 1000_00) {
                if (Boolean.TRUE.equals(cleanPositionStatusMap.get(deviceId))) {
                    // 置为false，下一次计算时就可以正常设置值到positionMap了
                    cleanPositionStatusMap.put(deviceId, Boolean.FALSE);
                    logger.info("置为false，下一次计算时就可以正常设置值到positionMap了");
                } else {
                    positionMap.put(deviceId, result);
                }
                postLocationRecord(deviceId, b562Result, r9250Result, result, resultTime,
                        focusCalculator.getAvgDelayMs(), focusCalculator.isShocked());
            }else{
                logger.error("融合值异常");
            }
        }
        focusCalculator.reset();
        calculatorMap.remove(deviceId);

    }

    private void postLocationRecord(String deviceId, double[] b562Result, double[] r9250Result, double[] result,
                                    LocalDateTime resultTime, int delay, boolean isShocked) {
        GnssCalcData locationRecord = new GnssCalcData();
        locationRecord.setCreatetime(resultTime);
        locationRecord.setUpdatetime(LocalDateTime.now()); //通过这里可以区分补传记录
        locationRecord.setDeviceid(deviceId);

        // 调用这个函数之前已判断是否为null
        locationRecord.setB562e(b562Result[0] * 10); //cm->mm
        locationRecord.setB562n(b562Result[1] * 10);
        locationRecord.setB562d(b562Result[2] * 10);

        if(r9250Result!=null) {
            locationRecord.setR9250e(r9250Result[0]);
            locationRecord.setR9250n(r9250Result[1]);
            locationRecord.setR9250d(r9250Result[2]);
        }

        locationRecord.setPps(delay);
        gnssCalcFilterService.calc(locationRecord, isShocked);
    }

    @Override
    public Tilt calcAvgTilt(String deviceId) {
        FocusCalculator1 focusCalculator = calculatorMap.get(deviceId);
        if (focusCalculator != null) {
            return focusCalculator.avgTilt();
        }
        return null;
    }

    /***
     * 清零：不需要，滤波窗口是按时间来选取历史数据的，只要超过4小时或一天，即使中间没有数据，也不会用
     * 超过filterCycle的数据

    public void cleanTiltByDeviceId(String deviceId) {
        logger.info("清零[{}]", deviceId);
        tiltMap.remove(deviceId);
        positionMap.remove(deviceId);
        cleanTiltStatusMap.put(deviceId, Boolean.TRUE);
        cleanPositionStatusMap.put(deviceId, Boolean.TRUE);
    }*/
}
