import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
import sys
import statistics
import math
import copy
import os,time
from scipy.stats import mode
from scipy.spatial import KDTree
from scipy.spatial import cKDTree
import matplotlib.font_manager as fm

#处理点云文件保存脚踝一下的部分点云
def dealPointData(pidPath,pidNewPath):
    # 读取 PLY 文件
    point_cloud = o3d.io.read_point_cloud(pidPath)
    # 移除离散点
    cl, ind = point_cloud.remove_statistical_outlier(nb_neighbors=20, std_ratio=0.06)
    point_cloud = point_cloud.select_by_index(ind)
    # 保存处理后的点云
    o3d.io.write_point_cloud(pidNewPath, point_cloud)
    data = np.loadtxt(pidNewPath)
    # 提取xyz坐标
    x = data[:, 0]
    y = data[:, 1]
    z = data[:, 2]
    # 创建一个布尔索引，筛选出在x-y平面内，半径在0.5范围内的点 (z >= 0.00) &
    mask = (z >=0) & (z <= 0.1) & (x**2 + y**2 <= 0.7**2)
    # 根据索引，保留符合条件的点
    filtered_data = data[mask]
    # 将保留的点云数据保存到新的文件
    np.savetxt(pidNewPath, filtered_data, delimiter=' ')


#计算点云的数据中，脚底和地板的切线位置
def boxCenter(pid):
    pidPath = "D:\\xyz\\" + str(pid) + "_point.xyz"
    pidNewPath = "D:\\xyz\\" + str(pid) + "_new.xyz"
    dealPointData(pidPath,pidNewPath)
    
    pcd = o3d.io.read_point_cloud(pidNewPath)
    allPoints = np.asarray(pcd.points)
    if len(allPoints) == 0:
        print("点云为空，无法计算均值。")
        return 0
    
    # 使用RANSAC算法提取平面
    plane_model, inliers = pcd.segment_plane(distance_threshold=0.00670, ransac_n=3, num_iterations=1000)
    inlier_cloud = pcd.select_by_index(inliers)
    # 获取被染成红色的点的坐标数组
    red_points = inlier_cloud.points
    red_points_np = np.asarray(red_points)
    # 获取 red_points_np 的 z 值数组
    z_red_values = red_points_np[:, 2]
    # 计算中位数
    medianRed = np.median(z_red_values)
    # 计算中位数向上的平均数
    meanRedUp = np.mean(z_red_values[z_red_values > medianRed])
    #计算中位数向下的平均数
    #meanRedDown = np.mean(z_red_values[z_red_values < medianRed])

    # Exclude ground points from allPoints
    foot_points_np = np.asarray([point for point in allPoints if point not in red_points_np])
    if len(foot_points_np) == 0 :
        print("脚部点云为空。")
        return 0
    #过滤掉地板，计算出脚底的点云数据
    points_np_foot = foot_points_np[(foot_points_np[:, 2] < 0.046) & (foot_points_np[:, 2] > meanRedUp)]
    # 计算平均值
    mean = np.mean(points_np_foot[:, 2])

    # 按照 Z 值进行排序
    sorted_points = points_np_foot[np.argsort(points_np_foot[:, 2])]
    # 获取 Z 值最低的前十个点
    if len(sorted_points) == 0:
        print("脚底板最低的点云为空")
        return 0
    try:
        lowest_points = sorted_points[10:20]
    except Exception as e:
        print("获取脚底板最低的倒数第十到20的点出现异常错误")
        return 0
    # 计算平均值
    meanLowEst = np.mean(lowest_points[:, 2]) + 1.05
    return meanLowEst