# -*- coding: utf-8 -*-
from decimal import Decimal
import os
import numpy as np
from collections import defaultdict
G = 9.7803267714 #重力加速度
IMU_Fre = 200    #IMU数据采样率

class IMUDataCheck:
    def __init__(self,data_path,settings,imuflag):
        self.IMUDataPath = data_path
        self.IMUData = None
        self.SINSFlagRes = defaultdict(list)
        self.ReadIMUData()
        self.FlagIMUData(settings)
        imuflag[0]=self.IMUDataJudge(settings)
    def ReadIMUData(self):
        IMU=[]
        if os.path.exists(self.IMUDataPath):
            with open(self.IMUDataPath,'r') as file: 
                IMU=file.readlines()
            i=0
            data=[]
            for line in IMU:
                IMU[i]=line.strip('\n')
                IMU[i]=IMU[i].split()
                data.append([str(IMU[i][2])+'.'+str(IMU[i][3]),float(IMU[i][4])*11460,float(IMU[i][5])*11460,float(IMU[i][6])*11460,float(IMU[i][7])*200,float(IMU[i][8])*200,float(IMU[i][9])*200])
                i=i+1
            self.IMUData=np.array(data,dtype=float)
            # self.IMUData[:][0].quantize(Decimal('0.000000')) 
        else:
            print("没有找到IMU文件")

    def FlagIMUData(self,settings):
        for i in range(3):
            if int(settings.SINS_TriAxis[i])==5 :
                Axis_Cou = i
                Axis_G = G
                break
            elif int(settings.SINS_TriAxis[i])==6 :
                Axis_Cou = i
                Axis_G = -G
                break

        for i in range(len(self.IMUData)):
            if self.IMUData[i][0]==self.IMUData[-1][0]:
                continue
            else:
                if (self.IMUData[i+1][0]-self.IMUData[i][0])<0.00002:
                    self.SINSFlagRes["RepTime"].append(self.IMUData[i][0])#存重复的前一个历元标签
                
                elif (self.IMUData[i+1][0]-self.IMUData[i][0]-1/IMU_Fre)>0.00002:
                    self.SINSFlagRes["MisTime"].append(self.IMUData[i][0])#存缺失的前一个历元标签
                
                if i<settings.SINS_StaticTime:
                    if Axis_Cou==0:
                        if abs(self.IMUData[i][4]-Axis_G)>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["X_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][5])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Y_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][6])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Z_AxisAccErr"].append(self.IMUData[i][0])
                    elif Axis_Cou==1:
                        if abs(self.IMUData[i][4])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["X_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][5]-Axis_G)>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Y_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][6])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Z_AxisAccErr"].append(self.IMUData[i][0])
                    elif Axis_Cou==2:
                        if abs(self.IMUData[i][4])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["X_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][5])>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Y_AxisAccErr"].append(self.IMUData[i][0])
                        if abs(self.IMUData[i][6]-Axis_G)>float(settings.SINS_MaxAccErr):
                            self.SINSFlagRes["Z_AxisAccErr"].append(self.IMUData[i][0])
    #0=SINS正常 1=SINS异常           
    def IMUDataJudge(self,settings):
        IMU_Continuity=0
        IMU_AxisHealth=0
        with open(settings.ResSavePath, "a") as f:
            f.write("# SINS异常检测信息："+"\n")
            f.write("  > IMU数据连续性："+"\n")
            if len(self.SINSFlagRes["MisTime"])>int(settings.SINS_MaxMisEpochNum):
                IMU_Continuity=1
                f.write("    IMU数据中断历元数过多"+"\n")
            else:
                f.write("    IMU数据连续性正常"+"\n")
            f.write("  > IMU三轴数据情况："+"\n")
            if len(self.SINSFlagRes["X_AxisAccErr"])!=0 or len(self.SINSFlagRes["Y_AxisAccErr"])!=0 or len(self.SINSFlagRes["Z_AxisAccErr"])!=0:
                IMU_AxisHealth=1
                f.write("    IMU三轴数据大小存在与实际朝向不符的情况"+"\n")
            else:
                 f.write("    IMU三轴数据正常"+"\n")
            f.write("  > 详细信息如下："+"\n")
            f.write("    重复历元标识(s)："+"\n")
            for i in range(len(self.SINSFlagRes["RepTime"])):
                f.write(self.SINSFlagRes["RepTime"][i])
            f.write("    缺失历元标识(s)："+"\n")
            for i in range(len(self.SINSFlagRes["MisTime"])):
                f.write(str(self.SINSFlagRes["MisTime"][i])+"\n")
            f.write("    三轴数据大小与实际不符历元标识(s)："+"\n")
            f.write("           X轴            Y轴            Z轴"+"\n")
            Axis_Num=[len(self.SINSFlagRes["X_AxisAccErr"]),len(self.SINSFlagRes["Y_AxisAccErr"]),len(self.SINSFlagRes["Z_AxisAccErr"])]
            for i in range(max(Axis_Num)):
                if i<len(self.SINSFlagRes["X_AxisAccErr"]) and i<len(self.SINSFlagRes["Y_AxisAccErr"]) and i<len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+str(self.SINSFlagRes["X_AxisAccErr"][i])+"  "+str(self.SINSFlagRes["Y_AxisAccErr"][i])+"  "+str(self.SINSFlagRes["Z_AxisAccErr"][i])+'\n')
                if i>=len(self.SINSFlagRes["X_AxisAccErr"]) and i<len(self.SINSFlagRes["Y_AxisAccErr"]) and i<len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+"             "+"  "+str(self.SINSFlagRes["Y_AxisAccErr"][i])+"  "+str(self.SINSFlagRes["Z_AxisAccErr"][i])+'\n')
                if i<len(self.SINSFlagRes["X_AxisAccErr"]) and i>=len(self.SINSFlagRes["Y_AxisAccErr"]) and i<len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+str(self.SINSFlagRes["X_AxisAccErr"][i])+"  "+"             "+"  "+str(self.SINSFlagRes["Z_AxisAccErr"][i])+'\n')
                if i<len(self.SINSFlagRes["X_AxisAccErr"]) and i<len(self.SINSFlagRes["Y_AxisAccErr"]) and i>=len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+str(self.SINSFlagRes["X_AxisAccErr"][i])+"  "+str(self.SINSFlagRes["Y_AxisAccErr"][i])+"  "+'\n')
                if i>=len(self.SINSFlagRes["X_AxisAccErr"]) and i>=len(self.SINSFlagRes["Y_AxisAccErr"]) and i<len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+"             "+"  "+"             "+"  "+str(self.SINSFlagRes["Z_AxisAccErr"][i])+'\n')
                if i>=len(self.SINSFlagRes["X_AxisAccErr"]) and i<len(self.SINSFlagRes["Y_AxisAccErr"]) and i>=len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+"             "+"  "+str(self.SINSFlagRes["Y_AxisAccErr"][i])+"  "+'\n')
                if i<len(self.SINSFlagRes["X_AxisAccErr"]) and i>=len(self.SINSFlagRes["Y_AxisAccErr"]) and i>=len(self.SINSFlagRes["Z_AxisAccErr"]):
                    f.write("      "+str(self.SINSFlagRes["X_AxisAccErr"][i])+'\n')
        if IMU_Continuity==1 or IMU_AxisHealth==1:
            print("IMU异常")
            return 1
        else:
            print("IMU正常")
            return 0