import subprocess
import sys
import json
import math
import time
import datetime

file_path = "/mnt/misc/single_dtof.flag"
power_mode = "low_power"

def call_adb_command(command):
    """Helper function to call adb command and capture its output."""
    try:
        print("RUN CMD:", command)
        result = subprocess.run(command, shell=True, check=True, text=True, capture_output=True)
        return result.stdout
    except subprocess.CalledProcessError as e:
        print(f"Error executing command: {command}\n{e.stderr}")
        return ""

def check_file_exists(file_path):
    # 构造检查文件存在的ADB命令
    command = f'adb shell ls {file_path}'
    result = call_adb_command(command)
    
    if 'No such file or directory' in result:
        return False
    else:
        return True

def calib_xtalk(modulename):
    print(datetime.datetime.now(), " RESULT:")
    # 先读取一下点tof值，确认附近没有障碍物
    command1 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_distance\"}'"
    result_str = call_adb_command(command1)
    print(datetime.datetime.now(), " RESULT:", result_str)
    ret_data = json.loads(result_str)
    if (ret_data["value"] < 30000):
        print(modulename, "ERROR: make there is no object neer tof during xtalk calib!")
        sys.exit(0)
    command = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"cali_xtalk\"}'"
    result_str = call_adb_command(command)
    ret_data = json.loads(result_str)
    if ret_data["ret"].lower() == "ok":
        print(modulename, "OK")
    else:
        print(modulename, "ERROR:", result_str)
    time.sleep(3)
    # while 1:
    #     command1 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_cali_status\"}'"
    #     result_str = call_adb_command(command1)
    #     print(datetime.datetime.now(), "RESULT:", result_str)
    #     ret_data = json.loads(result_str)
    #     if (ret_data["value"] == 2):
    #         break
    #     else:
    #         time.sleep(0.2)



def calib_ground_offset(modulename, depth):
    command2 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"cali_offset\",\"value\":" + str(23 + depth) + "}'"
    result_str = call_adb_command(command2)
    print("RESULT:", result_str)
    ret_data = json.loads(result_str)
    if ret_data["ret"].lower() == "ok":
        print(modulename, "OK")
    else:
        print(modulename, "ERROR:", result_str)
    time.sleep(5)
    # while 1:
    #     command1 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_cali_status\"}'"
    #     result_str = call_adb_command(command1)
    #     ret_data = json.loads(result_str)
    #     if (ret_data["value"] == 2):
    #         break
    #     else:
    #         time.sleep(0.2)

def calib_crossing(modulename, target):
    # 发送标定指令
    command2 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"cali_gradient\",\"value\":" + str(target) + "}'"
    result_str = call_adb_command(command2)
    print("RESULT:", result_str)
    ret_data = json.loads(result_str)
    if ret_data["ret"].lower() == "ok":
        print(modulename, "OK")
    else:
        print(modulename, "ERROR!", result_str)
    time.sleep(5)
    # while 1:
    #     command1 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_cali_status\"}'"
    #     result_str = call_adb_command(command1)
    #     ret_data = json.loads(result_str)
    #     if (ret_data["value"] == 2):
    #         break
    #     else:
    #         time.sleep(0.2)

def change_range_mode(modulename, rangemode):
    # 170是低光功率模式，85是高光功率模式
    command1 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"set_range_mode\",\"mode\":" + str(rangemode) + "}'"
    result_str = call_adb_command(command1)

def check_ground_offset(modulename, depth):
    command2 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_distance\"}'"
    result_str = call_adb_command(command2)
    print(f"RESULT: {result_str}")
    ret_data = json.loads(result_str)
    print(modulename, power_mode, "VALUE:", float(ret_data["value"]), ", DIFF:", float(ret_data["value"]) * 0.01 - (23 + depth), "mm")

def check_crossing(modulename, target):
    # 发送标定指令
    command2 = r"adb shell avacmd mcubase '{\"type\":\"tof\",\"module\":\"" + modulename + r"\",\"cmd\":\"get_distance\"}'"
    # 取两次数据，把之前缓存的脏数扔掉
    result_str = call_adb_command(command2)
    print("RESULT:", result_str)

    time.sleep(1)
    result_str = call_adb_command(command2)
    print("RESULT:", result_str)

    ret_data = json.loads(result_str)
    print(modulename, power_mode, "VALUE:", float(ret_data["value"]), ", DIFF:", float(ret_data["value"]) * 0.01 - target, "mm")

# 返回roll pitch角度，单位0.01度
def get_roll_pitch():
    command = r"adb shell avacmd clb '{\"type\":\"system\",\"cmd\":\"print_roll_pitch\"}'"
    result_str = call_adb_command(command)
    print("ROLL-PITCH:", result_str)
    ret_data = json.loads(result_str)
    return ret_data["roll"], ret_data["pitch"]

def main():
    global power_mode
    if len(sys.argv) < 3:
        print("Usage: 4cm_point_tof_test.py calib_xtalk calib_ground <value> calib_crossing <value>")
        return
    module_str = sys.argv[2]
    module_list = module_str.split(",")
    print(module_list)
    single_dtof = check_file_exists(file_path)

    if sys.argv[1] == 'calib_xtalk':
        print("start calibrate xtalk")
        for module in module_list:  
            calib_xtalk(module)
            time.sleep(0.5)
        sys.exit(0)
    elif sys.argv[1] == 'calib_ground_offset':
        print("start calibrate ground_offset")

        # 校验一下机器姿态，如果不平，说明要不没有做IMU标定，要不放的不平
        roll, pitch = get_roll_pitch()
        if abs(roll) >= 100 or abs(pitch) >= 100:
            print(module_str, "ERROR: fail to calibrate, the robot pose is not level, PLEASE make sure the robot is put level, and calibrate IMU before")
            sys.exit(0)
        # 先校验一下
        depth = 0  # 单位mm，默认为0，即平地标定时，机器驱动轮贴地平面，和点tof打的平面一样高，如果点tof打地平面更矮，则为正值
        if len(sys.argv) >=4:
            depth = float(sys.argv[3])
            print("DEPTH:", depth)
        for module in module_list:
            calib_ground_offset(module, depth)
            time.sleep(0.5)
        sys.exit(0)
    elif sys.argv[1] == 'calib_crossing':
        print("start calibrate crossing gradient")
        depth = 0  # 单位mm，默认为0，即平地标定时，机器驱动轮贴地平面，和点tof打的平面一样高，如果点tof打地平面更矮，则为正值
        if len(sys.argv) >=4:
            depth = float(sys.argv[3])
            print("DEPTH:", depth)
        # 这一步需要读取IMU数值,小轮支起的时候，roll应该在1度以内，pitch应该在12 - 14 度
        roll, pitch = get_roll_pitch()
        if abs(roll) >= 100 or  not (1200 <= pitch  and pitch <= 1400):
            print(module_str, "ERROR: the robot pose is not valid, PLEASE make sure the robot is stand in good way, pose:", roll, pitch)
            sys.exit(0)

        # 根据IMU读数计算理论的点tof距离，进行标定
        target = math.tan(pitch * 0.01 * 3.14159 / 180) * (163.9 + 149.16) + (23 - 7) + depth / math.cos(pitch * 0.01 * 3.14159 / 180)
        print("PITCH:", pitch, ", target distance:", target)
        for module in module_list:
            calib_crossing(module, target)
            time.sleep(0.5)
        sys.exit(0)
    elif sys.argv[1] == 'check_ground_offset':
        print("start check ground_offset")

        # 校验一下机器姿态，如果不平，说明要不没有做IMU标定，要不放的不平
        roll, pitch = get_roll_pitch()
        if abs(roll) >= 100 or abs(pitch) >= 100:
            print(module_str, "ERROR: fail to calibrate, the robot pose is not level, PLEASE make sure the robot is put level, and calibrate IMU before")
            sys.exit(0)
        # 先校验一下
        depth = 0  # 单位mm，默认为0，即平地标定时，机器驱动轮贴地平面，和点tof打的平面一样高，如果点tof打地平面更矮，则为正值
        if len(sys.argv) >=4:
            depth = float(sys.argv[3])
            print("DEPTH:", depth)
        for module in module_list:
            change_range_mode(module, 170)
        power_mode = "low_power"
        time.sleep(1)
        for module in module_list:
            check_ground_offset(module, depth)
        
        time.sleep(0.1)
        for module in module_list:
            change_range_mode(module, 85)
        power_mode = "high_power"
        time.sleep(1)
        for module in module_list:
            check_ground_offset(module, depth)
        sys.exit(0)
    elif sys.argv[1] == 'check_crossing':
        print("start check crossing distance")
        depth = 0  # 单位mm，默认为0，即平地标定时，机器驱动轮贴地平面，和点tof打的平面一样高，如果点tof打地平面更矮，则为正值
        if len(sys.argv) >=4:
            depth = float(sys.argv[3])
            print("DEPTH:", depth)
        # 这一步需要读取IMU数值,小轮支起的时候，roll应该在1度以内，pitch应该在12 - 14 度
        roll, pitch = get_roll_pitch()
        if abs(roll) >= 100 or  not (1200 <= pitch  and pitch <= 1400):
            print(module_str, "ERROR: the robot pose is not valid, PLEASE make sure the robot is stand in good way, pose:", roll, pitch)
            sys.exit(0)

        # 根据IMU读数计算理论的点tof距离，进行标定
        target = math.tan(pitch * 0.01 * 3.14159 / 180) * (163.9 + 149.16) + (23 - 7) + depth / math.cos(pitch * 0.01 * 3.14159 / 180)
        print("PITCH:", pitch, ", target distance:", target)

        for module in module_list:
            change_range_mode(module, 170)
        power_mode = "low_power"
        time.sleep(1)
        for module in module_list:
            check_crossing(module, target)
        
        time.sleep(0.1)
        for module in module_list:
            change_range_mode(module, 85)
        power_mode = "high_power"
        time.sleep(1)
        for module in module_list:
            check_crossing(module, target)
        sys.exit(0)
    else:
        print("ERROR: Invalid cmd", sys.argv)

if __name__ == '__main__':
    main()
