package com.imdroid.sideslope.calc;

import com.imdroid.common.util.ThreadManager;
import com.imdroid.secapi.dto.GnssCalcData;
import com.imdroid.secapi.dto.GnssGroupCalc;
import com.imdroid.secapi.dto.GnssGroupCalcMapper;
import com.imdroid.sideslope.bd.*;
import com.imdroid.sideslope.message.D341LocationMessage;
import com.imdroid.sideslope.sal.Device;
import com.imdroid.sideslope.sal.DeviceService;
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 javax.annotation.Resource;
import java.time.LocalDateTime;
import java.util.Arrays;
import java.util.List;
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, FocusCalculator> calculatorMap = new ConcurrentHashMap<>();

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

    @Autowired
    WarningService warningService;

    @Autowired
    GNSSCalcFilterService gnssCalcFilterService;

    @Resource(name = "local")
    private DeviceService deviceService;

    @Autowired
    GnssGroupCalcMapper groupCalcMapper;

    // 非线程安全，需加同步保护
    List<GnssGroupCalc> groupCalcList;

    synchronized GnssGroupCalc getGroupCalc(int groupId){
        if(groupCalcList == null){
            groupCalcList = groupCalcMapper.selectList(null);
        }
        if(groupCalcList != null){
            for(GnssGroupCalc groupCalc:groupCalcList){
                if(groupCalc.getId() == groupId) return groupCalc;
            }
        }
        return null;
    }

    /***
     * 处理本轮的固定解，求出重心位置。多线程调用，访问成员变量要考虑多线程安全
     * @param message d341报文
     * @param completeWhenIdle 是否根据空闲时间判断本轮结束。是则启动30s定时器；否则需显示调用SingleDone函数
     * @return
     */

    @Override
    public double[] calcSingle(D341LocationMessage message, boolean completeWhenIdle) {
        String deviceId = message.getId();
        Device device = deviceService.findByDeviceId(deviceId);
        if(device == null) return null;
        GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId());

        if(completeWhenIdle) resultOutputTimer(device, groupCalc, message.getCreateTime());

        //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
        FocusCalculator focusCalculator;
        if(groupCalc!=null && groupCalc.getVer() == 2){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator2());
        }
        else {
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator1());
        }

        // 读取惯导
        Tilt tilt = message.getTilt();
        focusCalculator.addTilt(tilt);
        if (logger.isDebugEnabled()) {
            logger.debug("测站" + deviceId + "惯导单次解析结果:{}", tilt);
        }

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

        // 单次b562
        double[] doubles = message.getB562_loc();//unit: mm
        focusCalculator.addXyz(doubles,message.getCreateTime());
        if (logger.isDebugEnabled()) {
            logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
        }
        return doubles;
    }

    @Override
    public void calSingleDone(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) {
        // executor已经在线程处理了
 /*       ThreadManager.getScheduledThreadPool().schedule(() -> {
            try {
                calCycleResult(device, groupCalc, resultTime);
            } catch (Exception e) {
                logger.error(e.toString());
            }
        },50, TimeUnit.MILLISECONDS);
*/
        calCycleResult(device, groupCalc, resultTime);
    }

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

        future = ThreadManager.getScheduledThreadPool().schedule(() -> {
            try {
                calCycleResult(device, groupCalc,date);
                // 清除统计
                device.clearStat();
            } catch (Exception e) {
                logger.error(e.toString());
            }
        },30, TimeUnit.SECONDS);
        timerMap.put(device.getDeviceId(), future);
    }

    private void calCycleResult(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) {
        String deviceId = device.getDeviceId();
        Integer tenantId = device.getTenantId();
        FocusCalculator focusCalculator = calculatorMap.get(deviceId);
        if(focusCalculator == null) return;

        // 数据处理：参考上次的位置，计算b562重心
        double[] referPos = focusCalculator.getReferPoint();
        double[] b562Result = focusCalculator.resultB562();
        /*double[] r9250Result = null; // 倾角转换成杆顶到杆底的位移
        double[] mergeResult = null;
        //判断 取到的b562  和上次融合坐标做判断，如果距离>200mm  不计算融合值
        if (referPos != null && b562Result!=null && FocusCalculator1.disXY(b562Result,referPos)<200){
            r9250Result = focusCalculator.result9250();
            mergeResult = focusCalculator.ekfResult(b562Result,r9250Result);//融合位置
        }*/
        Tilt tilt = focusCalculator.avgTilt();

        logger.info("测站 {} 的b562相对坐标重心:{}", deviceId, Arrays.toString(b562Result));
        logger.info("测站 {} 参考坐标:{}", deviceId, Arrays.toString(referPos));
        //logger.info("测站 {} 的惯导相对坐标:{}", deviceId, Arrays.toString(r9250Result));
        //logger.info("测站 {} 的相对坐标融合值:{}", deviceId, Arrays.toString(mergeResult));
        logger.info("测站 {} 的Tilt平均值:{}", deviceId, tilt);
/*
        if (mergeResult != null && b562Result != null) {
            //这里检查一下result，过滤1千米外非正常数据
            if(focusCalculator.dis(mergeResult,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, mergeResult);
                }
                postLocationRecord(deviceId, b562Result, r9250Result, mergeResult, resultTime,
                        focusCalculator.getAvgDelayMs(), focusCalculator.isShocked());
            }else{
                logger.error("融合值异常");
            }
        }*/
        if (b562Result != null) {
            GnssCalcData locationRecord = new GnssCalcData();
            locationRecord.setCreatetime(resultTime);
            locationRecord.setUpdatetime(LocalDateTime.now()); //通过这里可以区分补传记录
            locationRecord.setDeviceid(deviceId);

            // 本轮解算结果
            locationRecord.setB562e(b562Result[0]);
            locationRecord.setB562n(b562Result[1]);
            locationRecord.setB562d(b562Result[2]);
            // 本轮融合结果
            /*if(r9250Result!=null) {
                locationRecord.setR9250e(mergeResult[0]);
                locationRecord.setR9250n(mergeResult[1]);
                locationRecord.setR9250d(mergeResult[2]);
            }*/
            // pps
            locationRecord.setPps(focusCalculator.getAvgDelayMs());
            // 滤波后的结果
            gnssCalcFilterService.calc(device, groupCalc, locationRecord, referPos);
            
            // 记录本次位置，作为下次的参考
            // 算法1：以上轮位置作为参考，跟随变化速度快
            // 算法2：以滤波后的位置作为参考，跟随变化速度慢；如果倾角大于某一门限，则跟随上轮位置
            if(focusCalculator.getVer()==1 || focusCalculator.isShocked()) {
                focusCalculator.setReferPoint(b562Result);
            }
            else {
                FocusCalculator2 focusCalculator2 = (FocusCalculator2)focusCalculator;
                if(focusCalculator2.isOriginalReferPointSet()) {
                    if (locationRecord.getEnabled() && locationRecord.getRpose() != null) {
                        focusCalculator2.setReferPoint(new double[]{locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()});
                        logger.info("{} set reference pos to filter result:{},{},{}",
                                deviceId, locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd());
                    }
                    else{
                        logger.info("{} reference pos not changed",deviceId);
                    }
                }
                else{
                    // 查询设备有没有配置初始位置
                    if(device.getIPose()!=null){
                        focusCalculator2.setOriginalReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()});
                        logger.info("{} set reference pos to init pos:{},{},{}",
                                deviceId,device.getIPose(),device.getIPosn(),device.getIPosd());
                    }
                    else{
                        focusCalculator.setReferPoint(b562Result);
                        logger.info("{} set reference pos to b562 result",deviceId);
                    }
                }
            }

        }

        // 检查b562有效数，如果过少，产生告警
        warningService.checkB562Num(deviceId, tenantId, focusCalculator.getB562Stat());

        focusCalculator.reset();
    }

    public void refreshGroupCalc(){
        if(groupCalcList!=null) groupCalcList.clear();
        groupCalcList = groupCalcMapper.selectList(null);
        calculatorMap.clear();
        logger.info("group paras changed");
    }
}
