import json
import openpyxl
import pandas
import re
import csv
from PySide6.QtCore import Signal, QThread
import time
from codes.common import clibs


class CurrentDataProcess(QThread):
    output = Signal(str, str)

    def __init__(self, dir_path, proc, /):
        super().__init__()
        self.dir_path = dir_path
        self.proc = proc
        self.idx = 1

    def logger(self, level, module, content, color="black", error="", flag="both"):
        flag = "cursor" if level.upper() == "DEBUG" else "both"
        clibs.logger(level, module, content, color, flag, signal=self.output)
        if level.upper() == "ERROR":
            raise Exception(f"{error} | {content}")

    @clibs.handle_exception
    def initialization(self):
        _, data_files = clibs.traversal_files(self.dir_path, self.output)
        count, config_file = 0, None
        for data_file in data_files:
            filename = data_file.split("/")[-1]
            if re.match(".*\\.cfg", filename):
                config_file = data_file
                count += 1
            elif filename == "T_电机电流.xlsx":
                count += 1
            else:
                if not re.match("^j[1-7].*\\.data$", filename):
                    msg = f"不合规 {data_file}<br>"
                    msg += "所有数据文件必须以 j[1-7]_ 开头，以 .data 结尾，比如j1_abcdef.data，请检查整改后重新运行"
                    self.logger("ERROR", "current", msg, "red", "FilenameIllegal")

        if count != 2:
            msg = "需要有一个机型配置文件\"*.cfg\"，以及一个数据处理文件\"T_电机电流.xlsx\"表格，请检查整改后重新运行"
            self.logger("ERROR", "current", msg, "red", "FilenameIllegal")

        return data_files, config_file

    @clibs.handle_exception
    def current_max(self, data_files, rts):
        self.logger("INFO", "current", f"正在处理最大转矩值逻辑......")
        current = {1: [], 2: [], 3: [], 4: [], 5: [], 6: []}
        for data_file in data_files:
            if data_file.endswith(".data"):
                df = pandas.read_csv(data_file, sep="\t")
            else:
                continue

            self.logger("DEBUG", "current", f"正在处理 {data_file} ...")
            cols = len(df.columns)
            axis = int(data_file.split("/")[-1].split("_")[0].removeprefix("j"))
            rt = rts[axis-1]
            self.logger("DEBUG", "current", f"最大列数为 {cols}，{axis} 轴的额定转矩为 {rt}")

            col = df.columns.values[clibs.c_servo_trq-1]  # 获取 "device_servo_trq_feedback"
            c_max = df[col].abs().max()

            scale = 1000
            _ = abs(c_max/scale*rt)
            current[axis].append(_)
            self.logger("DEBUG", "current", f"{data_file}: {_:.2f}")
            self.logger("DEBUG", "current", f"获取到的列名为 {col}，最大转矩为 {_}")

            with open(data_file, "a+") as f_data:
                csv_writer = csv.writer(f_data, delimiter="\t")
                csv_writer.writerow([""] * (cols-1) + [_])

        for axis, cur in current.items():
            if not cur:
                continue
            else:
                _ = ""
                for value in cur:
                    _ += f"{value:.4f} "
                self.logger("INFO", "current", f"{axis}轴最大转矩数据：{_}")

        self.logger("DEBUG", "current", f"获取最大转矩值结束 current_max = {current}")
        self.logger("INFO", "current", f"最大转矩数据处理完毕......")
        return current

    @clibs.handle_exception
    def current_avg(self, data_files, rts):
        self.logger("INFO", "current", f"正在处理平均转矩值逻辑......")
        current = {1: [], 2: [], 3: [], 4: [], 5: [], 6: []}
        for data_file in data_files:
            if data_file.endswith(".data"):
                df = pandas.read_csv(data_file, sep="\t")
            else:
                continue

            self.logger("DEBUG", "current", f"正在处理 {data_file} ...")
            cols = len(df.columns)
            axis = int(data_file.split("/")[-1].split("_")[0].removeprefix("j"))
            rt = rts[axis-1]
            self.logger("DEBUG", "current", f"最大列数为 {cols}，{axis} 轴的额定转矩为 {rt}")

            col = df.columns.values[clibs.c_servo_trq-1]
            c_std = df[col].std()
            c_avg = df[col].mean()

            scale = 1000
            _ = (abs(c_avg)+c_std*3)/scale*rt
            current[axis].append(_)
            self.logger("DEBUG", "current", f"{data_file}: {_:.2f}")
            self.logger("DEBUG", "current", f"获取到的列名为 {col}，平均转矩为 {_}")

            with open(data_file, "a+") as f_data:
                csv_writer = csv.writer(f_data, delimiter="\t")
                csv_writer.writerow([""] * (cols-1) + [_])

        for axis, cur in current.items():
            if not cur:
                continue
            else:
                _ = ""
                for value in cur:
                    _ += f"{value:.4f} "
                self.logger("INFO", "current", f"{axis}轴平均转矩数据：{_}")

        self.logger("DEBUG", "current", f"获取平均转矩值结束 current_avg = {current}")
        self.logger("INFO", "current", f"平均转矩数据处理完毕......")
        return current

    @clibs.handle_exception
    def current_cycle(self, data_files, rrs, rts, params):
        result, hold, single, scenario, dur_time = None, [], [], [], 0
        for data_file in data_files:
            filename = data_file.split("/")[-1]
            if filename == "T_电机电流.xlsx":
                result = data_file
            elif re.match("j[1-7]_hold_.*\\.data", filename):
                hold.append(data_file)
            elif re.match("j[1-7]_s_.*\\.data", filename):
                scenario.append(data_file)
                dur_time = float(filename.split("_")[3])
            elif re.match("j[1-7]_.*\\.data", filename):
                single.append(data_file)

        clibs.stop, filename = True, result.split("/")[-1]
        self.logger("INFO", "current", f"正在打开文件 {filename}，这可能需要一些时间......", "blue")
        try:
            wb = openpyxl.load_workbook(result)
        except Exception as Err:
            self.logger("ERROR", "current", f"{filename}文件打开失败，可能是文件已损坏，确认后重新执行！<br>{Err}", "red", "CannotOpenFile")

        ws = wb["统计"]
        for idx in range(len(params)-1):
            row = idx + 2
            for col in range(2, 8):
                ws.cell(row=row, column=col).value = params[idx][col-2]
        ws.cell(row=1, column=1).value = params[-1]

        if hold:
            avg = self.current_avg(hold, rts)
            for axis, cur_value in avg.items():
                sht_name = f"J{axis}"
                wb[sht_name]["P4"].value = float(cur_value[0])

        if dur_time == 0:
            self.p_single(wb, single, rrs)
        else:
            self.p_scenario(wb, scenario, rrs, dur_time)

        self.logger("INFO", "current", f"正在保存文件 {filename}，这可能需要一些时间......", "blue")
        wb.save(result)
        wb.close()

    @clibs.handle_exception
    def find_point(self, data_file, df, flag, row_s, row_e, threshold, step, end_point, skip_scale, axis, seq):
        if flag == "lt":
            while row_e > end_point:
                speed_avg = df.iloc[row_s:row_e].abs().mean()
                if speed_avg < threshold:
                    row_e -= step
                    row_s -= step
                    continue
                else:
                    # one more time，如果连续两次 200 个点的平均值都大于 threshold，说明已经到了临界点了（其实也不一定，只不过相对遇到一次就判定临界点更安全一点点）
                    # 从实际数据看，这开逻辑很小概率能触发到
                    speed_avg = df.iloc[row_s-end_point*skip_scale:row_e-end_point*skip_scale].abs().mean()
                    if speed_avg < threshold:
                        self.logger("WARNING", "current", f"【lt】{axis} 轴第 {seq} 次查找数据可能有异常，row_s = {row_s}, row_e = {row_e}！", "purple")
                    return row_s, row_e
            else:
                self.logger("ERROR", "current", f"{data_file} 数据有误，需要检查，无法找到第 {seq} 个有效点......", "red", "AnchorNotFound")
        elif flag == "gt":
            while row_e > end_point:
                speed_avg = df.iloc[row_s:row_e].abs().mean()
                if speed_avg > threshold:
                    row_e -= step
                    row_s -= step
                    continue
                else:
                    # one more time，如果连续两次 200 个点的平均值都小于 threshold，说明已经到了临界点了（其实也不一定，只不过相对遇到一次就判定临界点更安全一点点）
                    # 从实际数据看，这开逻辑很小概率能触发到
                    speed_avg = df.iloc[row_s-end_point*skip_scale:row_e-end_point*skip_scale].abs().mean()
                    if speed_avg > threshold:
                        self.logger("WARNING", "current", f"【gt】{axis} 轴第 {seq} 次查找数据可能有异常，row_s = {row_s}, row_e = {row_e}！", "purple")
                    return row_s, row_e
            else:
                self.logger("ERROR", "current", f"{data_file} 数据有误，需要检查，无法找到第 {seq} 个有效点......", "red", "AnchorNotFound")

    @clibs.handle_exception
    def get_row_number(self, threshold, flag, df, row_s, row_e, axis):
        count_1, count_2 = 0, 0
        if flag == "start" or flag == "end":
            for number in df.iloc[row_s:row_e].abs():
                count_2 += 1
                if number > threshold:
                    count_1 += 1
                    if count_1 == 10:
                        return row_s + count_2 - 10
                else:
                    count_1 = 0
        elif flag == "middle":
            for number in df.iloc[row_s:row_e].abs():
                count_2 += 1
                if number < threshold:  # 唯一的区别
                    count_1 += 1
                    if count_1 == 10:
                        return row_s + count_2 - 10
                else:
                    count_1 = 0

        places = {"start": "起点", "middle": "中间点", "end": "终点"}  # 因为是终点数据，所以可能有异常
        self.logger("DEBUG", "current", f"{axis} 轴获取{places[flag]}数据 {row_e} 可能有异常，需关注！", "purple")
        return row_e

    @clibs.handle_exception
    def p_single(self, wb, single, rrs):
        # 1. 先找到第一个速度为零的点，数据从后往前找，一开始就是零的情况不予考虑
        # 2. 记录第一个点的位置，继续向前查找第二个速度为零的点，同理，一开始为零的点不予考虑
        # 3. 记录第二个点的位置，并将其中的数据拷贝至对应位置
        for data_file in single:
            axis = int(data_file.split("/")[-1].split("_")[0].removeprefix("j"))
            sht_name = f"J{axis}"
            ws = wb[sht_name]
            pandas.set_option("display.precision", 2)
            df_origin = pandas.read_csv(data_file, sep="\t")
            rr = rrs[axis-1]
            addition = 180 / 3.1415926 * 60 / 360 * rr

            col_names = list(df_origin.columns)
            df = df_origin[col_names[clibs.c_joint_vel-1]].multiply(addition)

            step = 50  # 步进值
            end_point = 200  # 有效数值的数目
            threshold = 5  # 200个点的平均阈值线
            skip_scale = 2
            row_start, row_middle, row_end = 0, 0, 0
            row_e = df.index[-1]
            row_s = row_e - end_point
            speed_avg = df.iloc[row_s:row_e].abs().mean()
            if speed_avg < threshold:
                # 第一次过滤：消除速度为零的数据，找到速度即将大于零的上升临界点
                row_s, row_e = self.find_point(data_file, df, "lt", row_s, row_e, threshold, step, end_point, skip_scale, axis, "pre-1")
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第二次过滤：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, "pre-2")
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第三次过滤：消除速度为零的数据，找到速度即将大于零的上升临界点
                row_s, row_e = self.find_point(data_file, df, "lt", row_s, row_e, threshold, step, end_point, skip_scale, axis, "pre-3")
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 正式第一次采集：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 1)
                row_end = self.get_row_number(threshold, "end", df, row_s, row_e, axis)
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 正式第二次采集：消除速度为零的数据，找到速度即将大于零的上升临界点
                row_s, row_e = self.find_point(data_file, df, "lt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 2)
                row_middle = self.get_row_number(threshold, "middle", df, row_s, row_e, axis)
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 正式第三次采集：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 3)
                row_start = self.get_row_number(threshold, "start", df, row_s, row_e, axis)
            elif speed_avg > threshold:
                # 第一次过滤：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, "pre-1")
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第二次过滤：消除速度为零的数据，找到速度即将大于零的上升临界点
                row_s, row_e = self.find_point(data_file, df, "lt", row_s, row_e, threshold, step, end_point, skip_scale, axis, "pre-2")
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第一次正式采集：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 1)
                row_end = self.get_row_number(threshold, "end", df, row_s, row_e, axis)
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第二次正式采集：消除速度为零的数据，找到速度即将大于零的上升临界点
                row_s, row_e = self.find_point(data_file, df, "lt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 2)
                row_middle = self.get_row_number(threshold, "middle", df, row_s, row_e, axis)
                row_e -= end_point*skip_scale
                row_s -= end_point*skip_scale
                # 第三次正式采集：消除速度大于零的数据，找到速度即将趋近于零的下降临界点
                row_s, row_e = self.find_point(data_file, df, "gt", row_s, row_e, threshold, step, end_point, skip_scale, axis, 3)
                row_start = self.get_row_number(threshold, "start", df, row_s, row_e, axis)

            self.logger("DEBUG", "current", f"{axis} 轴起点：{row_start}")
            self.logger("DEBUG", "current", f"{axis} 轴中间点：{row_middle}")
            self.logger("DEBUG", "current", f"{axis} 轴终点：{row_end}")
            self.logger("DEBUG", "current", f"{axis} 轴数据非零段点数：{row_middle-row_start+1}")
            self.logger("DEBUG", "current", f"{axis} 轴数据为零段点数：{row_end-row_middle+1}")
            if abs(row_end+row_start-2*row_middle) > 1000:
                self.logger("DEBUG", "current", f"{axis} 轴数据占空比异常！", "purple")

            data, first_c, second_c, third_c, fourth_c = [], clibs.c_joint_vel-1, clibs.c_servo_trq-1, clibs.c_sensor_trq-1, clibs.c_estimate_trans_trq-1
            for row in range(row_start, row_end+1):
                data.append(df_origin.iloc[row, first_c])
                data.append(df_origin.iloc[row, second_c])
                data.append(df_origin.iloc[row, third_c])
                data.append(df_origin.iloc[row, fourth_c])

            i = 0
            for row in ws.iter_rows(min_row=2, min_col=2, max_row=150000, max_col=5):
                for cell in row:
                    try:
                        if i % 4 == 0:
                            ws.cell((i//4)+2, 1).value = float(((i//4)+1)/1000)
                        _ = f"{data[i]:.2f}"
                        cell.value = float(_)
                        i += 1
                    except Exception:
                        if i % 4 == 0:
                            ws.cell((i//4)+2, 1).value = None
                        cell.value = None
                        i += 1

    @clibs.handle_exception
    def p_scenario(self, wb, scenario, rrs, dur_time):
        self.logger("INFO", "current", f"本次处理的是电机电流场景数据，场景运动周期为 {dur_time}s", "blue")
        for data_file in scenario:
            cycle = 0.001
            axis = int(data_file.split("/")[-1].split("_")[0].removeprefix("j"))
            sht_name = f"J{axis}"
            ws = wb[sht_name]
            pandas.set_option("display.precision", 2)
            df_origin = pandas.read_csv(data_file, sep="\t")
            rr = rrs[axis-1]
            addition = 180 / 3.1415926 * 60 / 360 * rr

            col_names = list(df_origin.columns)
            df = df_origin[col_names[clibs.c_joint_vel-1]].multiply(addition)

            row_start = 3000
            row_end = row_start + int(dur_time/cycle)
            if row_end > df.index[-1]:
                self.logger("ERROR", "current", f"位置超限：{data_file} 共有 {df.index[-1]} 条数据，无法取到第 {row_end} 条数据，需要确认场景周期时间...", "blue", "DataOverLimit")

            data, first_c, second_c, third_c, fourth_c = [], clibs.c_joint_vel-1, clibs.c_servo_trq-1, clibs.c_sensor_trq-1, clibs.c_estimate_trans_trq-1
            for row in range(row_start, row_end+1):
                data.append(df_origin.iloc[row, first_c])
                data.append(df_origin.iloc[row, second_c])
                data.append(df_origin.iloc[row, third_c])
                data.append(df_origin.iloc[row, fourth_c])

            i = 0
            for row in ws.iter_rows(min_row=2, min_col=2, max_row=250000, max_col=5):
                for cell in row:
                    try:
                        if i % 4 == 0:
                            ws.cell((i//4)+2, 1).value = float(((i//4)+1)/1000)
                        _ = f"{data[i]:.2f}"
                        cell.value = float(_)
                        i += 1
                    except Exception:
                        cell.value = None
                        if i % 4 == 0:
                            ws.cell((i//4)+2, 1).value = None
                        i += 1

    @clibs.handle_exception
    def get_configs(self, config_file):
        try:
            if re.match("^[NXEC]B.*", config_file.split("/")[-1]):
                robot_type = "工业"
            else:
                robot_type = "协作"

            with open(config_file, mode="r", encoding="utf-8") as f_config:
                configs = json.load(f_config)

            version = configs["VERSION"]
            sc = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]  # 采样周期，sc for sample cycle
            r_rrs = configs["TRANSMISSION"]["REDUCTION_RATIO_NUMERATOR"]  # 减速比，rr for reduction ratio
            m_avs = configs["MOTION"]["JOINT_MAX_SPEED"]
            m_stall_ts = configs["MOTOR"]["STALL_TORQUE"]  # 电机堵转转矩
            m_rts = configs["MOTOR"]["RATED_TORQUE"]  # 电机额定转矩rt for rated torque
            m_max_ts = configs["MOTOR"]["PEAK_TORQUE"]  # 电机峰值转矩
            m_r_rpms = configs["MOTOR"]["RATED_SPEED"]  # 电机额定转速
            m_max_rpms = configs["MOTOR"]["MAX_SPEED"]  # 电机最大转速
            r_max_sst = configs["TRANSMISSION"]["MAX_TORQUE_FOR_START_AND_STOP"]  # 减速器最大启停转矩，sst for start and stop torque
            r_max_t = configs["TRANSMISSION"]["MAX_PEAK_TORQUE"]  # 减速器瞬时最大转矩
            r_avg_t = configs["TRANSMISSION"]["MAX_AVERAGE_TORQUE"]  # 减速器平均负载转矩允许最大值

            self.logger("INFO", "current", f"get_configs: 机型文件版本 {config_file}_{version}")
            self.logger("INFO", "current", f"get_configs: 减速比 {r_rrs}")
            self.logger("INFO", "current", f"get_configs: 额定转矩 {m_rts}")
            self.logger("INFO", "current", f"get_configs: 最大角速度 {m_avs}")
            return sc, r_rrs, m_avs, m_stall_ts, m_rts, m_max_ts, m_r_rpms, m_max_rpms, r_max_sst, r_max_t, r_avg_t, robot_type
        except Exception as Err:
            self.logger("ERROR", "current", f"get_config: 无法打开 {config_file}，或获取配置文件参数错误 {Err}", "red", "OpenFileError")

    @clibs.handle_exception
    def processing(self):
        time_start = time.time()
        clibs.running[self.idx] = 1

        data_files, config_file = self.initialization()
        params = self.get_configs(config_file)
        rts, rrs = params[4], params[1]
        if self.proc == "最大值":
            self.current_max(data_files, rts)
        elif self.proc == "平均值":
            self.current_avg(data_files, rts)
        elif self.proc == "周期":
            self.current_cycle(data_files, rrs, rts, params)

        self.logger("INFO", "current", "-"*60 + "<br>全部处理完毕<br>", "purple")
        time_total = time.time() - time_start
        msg = f"数据处理时间：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s\n"
        self.logger("INFO", "current", msg)