package com.imdroid.sideslope.calc;

import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
import com.imdroid.common.util.DataTypeUtil;
import com.imdroid.common.util.ThreadManager;
import com.imdroid.secapi.dto.*;
import com.imdroid.sideslope.bd.*;
import com.imdroid.sideslope.message.D331RtcmMessage;
import com.imdroid.sideslope.message.D341LocationMessage;
import com.imdroid.sideslope.service.Device;
import com.imdroid.sideslope.service.DeviceService;
import com.imdroid.sideslope.server.DeviceChannel;
import com.imdroid.sideslope.server.OnlineChannels;
import com.imdroid.sideslope.service.GnssSingleBufferService;
import com.imdroid.sideslope.service.GroupParaService;
import com.imdroid.sideslope.service.WarningService;
import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
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.List;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ScheduledFuture;
import java.util.concurrent.TimeUnit;
/**
 * @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;

    @Autowired
    private DeviceService deviceService;

    @Autowired
    GroupParaService groupParaService;
    @Autowired
    GnssCalcDataMapper dataMapper;

    @Autowired
    private GnssSingleBufferService gnssSingleDataService;


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

    @Override
    public void calcSingle(D341LocationMessage message, boolean completeWhenIdle) {
        String deviceId = message.getId();
        Device device = deviceService.findByDeviceId(deviceId);
        if(device == null) return;
        GnssGroupCalc groupCalc = groupParaService.getCalcGroup(device.getCalcGroupId());
        if(groupCalc==null) return;
        device.setB562AsCalc(groupCalc.getVer()!=3);

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

        //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
        FocusCalculator focusCalculator;
        if(groupCalc.getVer() == 6){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50));
        }
        else if(groupCalc.getVer() == 5){
            focusCalculator = calculatorMap.get(deviceId);
            if(focusCalculator ==null) {
                Short removeRate = groupCalc.getRemove_rate();
                if (removeRate == null) removeRate = 20;
                focusCalculator = new FocusCalculator5(deviceId, 50, removeRate);
                calculatorMap.put(deviceId,focusCalculator);
            }
        }
        else if(groupCalc.getVer() == 4){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
        }
        else if(groupCalc.getVer() == 3){
            Device bsDevice = deviceService.findByDeviceId(device.getParentId());
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3(bsDevice));
        }
        else if(groupCalc.getVer() == 2){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator2());
        }
        else {
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator1());
        }

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

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

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

        // 单次GGA
        Gga gga = message.getGga();
        if(gga != null) {
            focusCalculator.addGGA(gga);
            logger.debug("测站{}的gga单次解析结果:{},{},{},{}",deviceId,
                        gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
            if(groupCalc.getVer() == 3 && focusCalculator.isJump()){
                logger.info("{}发生周跳",deviceId);
                hardResetDevice(deviceId);
                device.setJumpCount(device.getJumpCount()+1);
            }
        }

        // 若是该设备开启了日志记录，则保存单次解析的数据
        if(device.getLoggingmode() == GnssDevice.LOGGING_MODE_FULL){
            // 保存单次解析的原始数据，受 loggingmode 字段控制
            GnssSingleData gnssSingleData = new GnssSingleData();
            gnssSingleData.setDeviceid(device.getDeviceId());
            gnssSingleData.setCreatetime(LocalDateTime.now());
            gnssSingleData.setModel(device.getModel());
            if((device.getModel() == GnssDevice.MODEL_G505) && (doubles !=null)){
                gnssSingleData.setX(doubles[0]);
                gnssSingleData.setY(doubles[1]);
                gnssSingleData.setZ(doubles[2]);
                gnssSingleData.setStatus((int) doubles[3]);
                gnssSingleDataService.addData(gnssSingleData);
            }
            else if((device.getModel() == GnssDevice.MODEL_G510) && (gga !=null)){
                gnssSingleData.setX(gga.getLatitude());
                gnssSingleData.setY(gga.getLongitude());
                gnssSingleData.setZ(gga.getAltitude());
                gnssSingleData.setStatus(gga.getQuality());
                gnssSingleDataService.addData(gnssSingleData);
            }
        }

    }

    @Override
    public void calcSingle(D331RtcmMessage message, boolean completeWhenIdle) {
        String deviceId = message.getId();
        Device device = deviceService.findByDeviceId(deviceId);
        if(device == null) return;
        GnssGroupCalc groupCalc = groupParaService.getCalcGroup(device.getCalcGroupId());
        if(groupCalc==null) return;
        device.setB562AsCalc(groupCalc.getVer()!=3);

        //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
        FocusCalculator focusCalculator;
        if(groupCalc.getVer() == 6){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50));
        }
        else if(groupCalc.getVer() == 5){
            focusCalculator = calculatorMap.get(deviceId);
            if(focusCalculator ==null) {
                Short removeRate = groupCalc.getRemove_rate();
                if (removeRate == null) removeRate = 20;
                focusCalculator = new FocusCalculator5(deviceId, 50, removeRate);
                calculatorMap.put(deviceId,focusCalculator);
            }
        }
        else if(groupCalc.getVer() == 4){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
        }
        else if(groupCalc.getVer() == 3){
            Device bsDevice = deviceService.findByDeviceId(device.getParentId());
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3(bsDevice));
        }
        else if(groupCalc.getVer() == 2){
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator2());
        }
        else {
            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator1());
        }

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

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

        // 单次GGA
        Gga gga = message.getGga();
        if(gga != null) {
            focusCalculator.addGGA(gga);
            logger.debug("测站{}的gga单次解析结果:{},{},{},{}",deviceId,
                    gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
            if(groupCalc.getVer() == 3 && focusCalculator.isJump()){
                logger.info("{}发生周跳",deviceId);
                hardResetDevice(deviceId);
                device.setJumpCount(device.getJumpCount()+1);
            }

            if(completeWhenIdle &&
                    (gga.getQuality()==Gga.FIX_RESULT || gga.getQuality()==Gga.FLOAT_RESULT)) {
                resultOutputTimer(device, groupCalc, message.getCreateTime());
            }

        }

        // 若是该设备开启了日志记录，则保存单次解析的数据
        if(device.getLoggingmode() == GnssDevice.LOGGING_MODE_FULL){
            // 保存单次解析的原始数据，受 loggingmode 字段控制
            GnssSingleData gnssSingleData = new GnssSingleData();
            gnssSingleData.setDeviceid(device.getDeviceId());
            gnssSingleData.setCreatetime(LocalDateTime.now());
            gnssSingleData.setModel(device.getModel());
            if(gga !=null){
                gnssSingleData.setX(gga.getLatitude());
                gnssSingleData.setY(gga.getLongitude());
                gnssSingleData.setZ(gga.getAltitude());
                gnssSingleData.setStatus(gga.getQuality());
                gnssSingleDataService.addData(gnssSingleData);
            }
        }

    }

    @Override
    public void calSingleDone(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) {
        ScheduledFuture<?> future = timerMap.get(device.getDeviceId());
        if (future != null && !future.isDone()) {
            future.cancel(true);
            future = null;
        }
        if(null == groupCalc){
            groupCalc = groupParaService.getCalcGroup(device.getCalcGroupId());
            if(groupCalc == null) return;
        }
        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);
            } catch (Exception e) {
                logger.error(e.toString());
            }
        },90, TimeUnit.SECONDS);
        timerMap.put(device.getDeviceId(), future);
    }

    private void calCycleResult(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) {
        String deviceId = device.getDeviceId();
        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.debug("测站 {} 的b562相对坐标重心:{}", deviceId, Arrays.toString(b562Result));
        logger.debug("测站 {} 参考坐标:{}", deviceId, Arrays.toString(referPos));
        //logger.info("测站 {} 的惯导相对坐标:{}", deviceId, Arrays.toString(r9250Result));
        //logger.info("测站 {} 的相对坐标融合值:{}", deviceId, Arrays.toString(mergeResult));
        logger.debug("测站 {} 的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());
            // 滤波后的结果
            double[] latestRpos = getLatestRpos(deviceId);
            if(latestRpos != null) {
                gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
            }
            else {
                if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5){
                    gnssCalcFilterService.calc(device, groupCalc, locationRecord, null);
                }
                else {
                    gnssCalcFilterService.calc(device, groupCalc, locationRecord, focusCalculator.getReferPoint());
                }
            }

            // 如果本次结果有跳变，则停止推送并产生严重告警
            //更新有效解时间
            if(locationRecord.getEnabled()){
                if(device.getLastValidCalcDataTime()==null || device.getLastValidCalcDataTime().isBefore(resultTime)){
                    device.setLastValidCalcDataTime(resultTime);
                }
                if(latestRpos!=null){
                    warningService.checkFilteredResultJump(latestRpos, locationRecord);
                }
            }

            // 记录本次位置，作为下次的参考
            if(focusCalculator.getVer()==6) {
                if(locationRecord.getRpose()!=null) {
                    focusCalculator.setReferPoint(
                            new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});
                }
                //检查过去24小时是否有有效解，如果没有，用24小时固定解求一个均值作为
                else if(focusCalculator.getReferPoint() == null){
                    if(latestRpos != null) focusCalculator.setReferPoint(latestRpos);
                    else {
                        double[] avgEnu = gnssCalcFilterService.calcFilterValueByLastDay(locationRecord);
                        logger.debug("{} calc 24 hours filtered pos", deviceId);
                        if (avgEnu != null) {
                            ((FocusCalculator6) focusCalculator).setOriginalReferPoint(avgEnu);
                        }
                    }
                }
            }
            else if(focusCalculator.getVer()==4) {
                if(locationRecord.getRpose()!=null) {
                    focusCalculator.setReferPoint(
                            new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});
                }
                else if(focusCalculator.getReferPoint() == null){
                    if(latestRpos != null) focusCalculator.setReferPoint(latestRpos);
                    else focusCalculator.setReferPoint(b562Result);
                }
            }
            else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5) {
                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.debug("{} set reference pos to filter result:{},{},{}",
                                deviceId, locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd());
                    }
                    else{
                        logger.debug("{} reference pos not changed",deviceId);
                    }
                }
                else{
                    // 查询设备有没有配置初始位置
                    if(device.getIPose()!=null){
                        focusCalculator2.setOriginalReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()});
                        logger.debug("{} set reference pos to init pos:{},{},{}",
                                deviceId,device.getIPose(),device.getIPosn(),device.getIPosd());
                    }
                    else{
                        focusCalculator.setReferPoint(b562Result);
                        logger.debug("{} set reference pos to b562 result",deviceId);
                    }
                }
            }

        }

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

        focusCalculator.reset();
    }

    public void refreshDeviceCalc(String deviceId){
        Device device = deviceService.findByDeviceId(deviceId);
        if(device == null) return;

        if(device.getDeviceType() == Device.DEVICE_BASE_STATION){
            List<Device> deviceList = deviceService.findByParentId(deviceId);
            for(Device d:deviceList){
                calculatorMap.remove(d.getDeviceId());
            }
            logger.info("base station device paras changed");
        }
        else{
            calculatorMap.remove(deviceId);
        }
        logger.info("device paras changed");
    }

    double[] getLatestRpos(String deviceId){
        QueryWrapper<GnssCalcData> queryWrapper = new QueryWrapper<>();
        queryWrapper.eq("deviceid",deviceId);
        queryWrapper.ge("createtime",LocalDateTime.now().minusHours(12));
        queryWrapper.isNotNull("rpose");
        queryWrapper.eq("enabled",true);
        queryWrapper.orderByDesc("createtime");
        queryWrapper.last("limit 1");
        GnssCalcData gnssCalcData = dataMapper.selectOne(queryWrapper);
        if(gnssCalcData != null){
            logger.debug("{} getLatestPos", deviceId);
            return new double[]{gnssCalcData.getRpose(),gnssCalcData.getRposn(),gnssCalcData.getRposd()};
        }
        return null;
    }

    void hardResetDevice(String deviceId){
        String resetCmd = "42 4b cc 17 26 04 00 00";
        DeviceChannel deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId);
        if(deviceChannel == null || !deviceChannel.isOnline()) {
            deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId);
        }
        //if(deviceChannel!=null && deviceChannel.isOnline()){
        if(deviceChannel!=null && deviceChannel.isOnline()){
            // send command
            ByteBuf buf = Unpooled.buffer();
            byte[] data = DataTypeUtil.hexStringToBytes(resetCmd);
            logger.info("send hard reset command:{}", resetCmd);
            buf.writeBytes(data);
            deviceChannel.writeAndFlush(buf);
        }
    }

}
