import MilCon
import typing
import math


class _HPos:
    x: str
    y: str
    z: str

class RelativePos:
    def __init__(self, x: str | int | float = "~", y: str | int | float = "~", z: str | int | float = "~"):
        self.x: str = str(x) if isinstance(x, (int, float)) else x
        self.y: str = str(y) if isinstance(y, (int, float)) else y
        self.z: str = str(z) if isinstance(z, (int, float)) else z
    def __str__(self):
        return f"{self.x} {self.y} {self.z}"
    @classmethod
    def FromString(cls, s: str):
        parts = s.split()
        if len(parts) != 3 or not all(p.startswith('~') for p in parts): raise ValueError("Catch A Error As RelativePos ~x ~y ~z")
        return cls(*parts)
    def ToAbsolute(self, reference: _HPos=None):
        def ParseCoord(coord: str, ref):
            if coord.startswith('~'): return ref + (float(coord[1:]) if len(coord) > 1 else 0.0)
            else: return float(coord)
        ref_x = reference.x if reference and reference.x.startswith('~') else 0.0
        ref_y = reference.y if reference and reference.y.startswith('~') else 0.0
        ref_z = reference.z if reference and reference.z.startswith('~') else 0.0
        abs_x = ParseCoord(self.x, ref_x)
        abs_y = ParseCoord(self.y, ref_y)
        abs_z = ParseCoord(self.z, ref_z)
        return RelativePos(abs_x, abs_y, abs_z)

class LocalCoordinatesPos:
    def __init__(self, x: str | int | float = "^", y: str | int | float = "^", z: str | int | float = "^"):
        self.x: str = str(x) if isinstance(x, (int, float)) else x
        self.y: str = str(y) if isinstance(y, (int, float)) else y
        self.z: str = str(z) if isinstance(z, (int, float)) else z
    def __str__(self):
        return f"{self.x} {self.y} {self.z}"
    @classmethod
    def FromString(cls, s: str):
        parts = s.split()
        if len(parts) != 3 or not all(p.startswith('^') for p in parts): raise ValueError("Catch A Error As LocalCoordinatesPos ^x ^y ^z")
        return cls(*parts)
    def ToAbsolute(self, reference: _HPos=None):
        def ParseCoord(coord: str, ref):
            if coord.startswith('^'): return ref + (float(coord[1:]) if len(coord) > 1 else 0.0)
            else: return float(coord)
        ref_x = reference.x if reference and reference.x.startswith('^') else 0.0
        ref_y = reference.y if reference and reference.y.startswith('^') else 0.0
        ref_z = reference.z if reference and reference.z.startswith('^') else 0.0
        abs_x = ParseCoord(self.x, ref_x)
        abs_y = ParseCoord(self.y, ref_y)
        abs_z = ParseCoord(self.z, ref_z)
        return LocalCoordinatesPos(abs_x, abs_y, abs_z)

class Delta:
    def __init__(self, x: int = 0, y: int = 0, z: int = 0):
        self.x, self.y, self.z = x, y, z
    def __str__(self):
        return f"{self.x} {self.y} {self.z}"
class Command():
    def __init__(self, command):
        self.command = command
    def __str__(self):
        return self.command
class ExecuteSubCommandRule(typing.TypedDict):
    mode: MilCon._execute_sub_commands
    text: Command

def _Bool2Str(value: bool):
    return "true" if value else "false"
def _WithValue(value: str | int):
    return f" {value}" if value else ""

def _Pos1ToPos2(pos1: RelativePos, pos2: RelativePos, radios: int):
    abs_pos1 = pos1.ToAbsolute()
    abs_pos2 = pos2.ToAbsolute()
    x1, y1, z1 = float(abs_pos1.x), float(abs_pos1.y), float(abs_pos1.z)
    x2, y2, z2 = float(abs_pos2.x), float(abs_pos2.y), float(abs_pos2.z)
    delta_x = (x2 - x1) / (radios + 1)
    delta_y = (y2 - y1) / (radios + 1)
    delta_z = (z2 - z1) / (radios + 1)
    points = []
    for i in range(1, radios + 1):
        x = x1 + delta_x * i
        y = y1 + delta_y * i
        z = z1 + delta_z * i
        points.append(RelativePos(x, y, z))
    return points

def _Pos1ToPos2WithPoints(radios: int, radian: int, pos1: RelativePos, pos2: RelativePos, *poses: RelativePos):
    # 参数验证
    if radios <= len(poses) + 2:
        raise ValueError("radios must be greater than len(poses) + 2")
    if radian < 0:
        raise ValueError("radian must be non-negative")
    
    # 转换为绝对坐标
    abs_pos1 = pos1.ToAbsolute()
    abs_poses = [p.ToAbsolute() for p in poses]
    abs_pos2 = pos2.ToAbsolute()
    
    # 所有点按顺序排列
    all_points = [abs_pos1] + abs_poses + [abs_pos2]
    num_segments = len(all_points) - 1
    
    # 计算每段的点数
    points_per_segment = (radios - num_segments + 1) // num_segments
    remainder = (radios - num_segments + 1) % num_segments
    
    # 生成点
    points = []
    for i in range(num_segments):
        start = all_points[i]
        end = all_points[i + 1]
        
        # 当前段的点数
        current_points = points_per_segment + (1 if i < remainder else 0)
        
        # 计算增量
        delta_x = (float(end.x) - float(start.x)) / (current_points + 1)
        delta_y = (float(end.y) - float(start.y)) / (current_points + 1)
        delta_z = (float(end.z) - float(start.z)) / (current_points + 1)
        
        # 生成当前段的点
        for j in range(1, current_points + 1):
            x = float(start.x) + delta_x * j
            y = float(start.y) + delta_y * j
            z = float(start.z) + delta_z * j
            
            # 如果 radian 不为 0 且 poses 存在，则在中间点附近向外偏移
            if radian != 0 and len(poses) > 0 and i < len(poses):
                # 计算当前点与中间点的距离比例 (0-1)
                t = j / (current_points + 1)
                
                # 使用正弦函数生成弧形偏移 (0-π)
                offset = radian * math.sin(t * math.pi)
                
                # 计算3D垂直方向
                # 获取前一个点和后一个点
                prev_point = all_points[i]
                next_point = all_points[i+1]
                
                # 计算线段方向向量
                direction = (
                    float(next_point.x) - float(prev_point.x),
                    float(next_point.y) - float(prev_point.y),
                    float(next_point.z) - float(prev_point.z)
                )
                
                # 计算垂直方向 (使用简单的垂直向量)
                # 这里我们选择一个与方向向量垂直的向量
                if direction[0] != 0 or direction[1] != 0:
                    # 如果方向向量不是纯z轴方向，我们可以用叉积计算垂直向量
                    perpendicular = (
                        -direction[1],
                        direction[0],
                        0
                    )
                else:
                    # 如果是纯z轴方向，用x轴作为垂直方向
                    perpendicular = (1, 0, 0)
                
                # 归一化垂直向量
                length = math.sqrt(sum(p*p for p in perpendicular))
                if length > 0:
                    perpendicular = (
                        perpendicular[0]/length,
                        perpendicular[1]/length,
                        perpendicular[2]/length
                    )
                
                # 应用偏移
                x += offset * perpendicular[0]
                y += offset * perpendicular[1]
                z += offset * perpendicular[2]
            
            points.append(RelativePos(x, y, z))
    
    return points

def _Pos1ToPos2DrawPi(pos1: RelativePos, pos2: RelativePos, points: int) -> typing.List[RelativePos]:
    """
    Generate a list of points forming a circle with pos1 as the center,
    pos2 as a point on the circumference, and return num_points including pos1 at start and end.
    
    Args:
        pos1: Center of the circle (absolute coordinates)
        pos2: Point on the circumference (absolute coordinates)
        num_points: Number of points to generate (including start/end point)
    
    Returns:
        List of RelativePos points forming the circle, starting and ending with pos1
    """
    if points < 2:
        raise ValueError("Number of points must be at least 2 (including start/end)")
    
    # Convert positions to absolute float values
    center_x = float(pos1.x)
    center_y = float(pos1.y)
    center_z = float(pos1.z)
    
    point_x = float(pos2.x)
    point_y = float(pos2.y)
    point_z = float(pos2.z)
    
    # Calculate radius as distance from pos1 to pos2
    dx = point_x - center_x
    dy = point_y - center_y
    dz = point_z - center_z
    radius = (dx**2 + dy**2 + dz**2) ** 0.5
    
    # Determine the plane of the circle
    # Assume the circle is in the x-z plane if dy is zero, otherwise use x-y plane
    # This is a simplification and may need adjustment based on actual requirements
    points = []
    
    for i in range(points):
        angle = 2 * math.pi * i / (points - 1)
        
        # Simplified 2D circle in x-z plane (y remains constant)
        x = center_x + radius * math.cos(angle)
        z = center_z + radius * math.sin(angle)
        y = center_y  # y coordinate remains the same as center
        
        points.append(RelativePos(x, y, z))
    
    return points

'''def _Pos1ToPos2WithPoints(radios: int, pos1: RelativePos, pos2: RelativePos, *poses: RelativePos):
    if radios <= len(poses) + 2:
        raise ValueError("radios must be greater than len(poses) + 2")
    abs_pos1 = pos1.ToAbsolute()
    abs_poses = [p.ToAbsolute() for p in poses]
    abs_pos2 = pos2.ToAbsolute()
    all_points = [abs_pos1] + abs_poses + [abs_pos2]
    num_segments = len(all_points) - 1
    points_per_segment = (radios - num_segments + 1) // num_segments
    remainder = (radios - num_segments + 1) % num_segments
    points = []
    for i in range(num_segments):
        start = all_points[i]
        end = all_points[i + 1]
        current_points = points_per_segment + (1 if i < remainder else 0)
        delta_x = (float(end.x) - float(start.x)) / (current_points + 1)
        delta_y = (float(end.y) - float(start.y)) / (current_points + 1)
        delta_z = (float(end.z) - float(start.z)) / (current_points + 1)
        for j in range(1, current_points + 1):
            x = float(start.x) + delta_x * j
            y = float(start.y) + delta_y * j
            z = float(start.z) + delta_z * j
            points.append(RelativePos(x, y, z))
    return points'''