import pygame
import math
import numpy as np
import random
from utils.cv_marker import cap_and_mark
from utils.stack_exe import Stackbot
import cv2


pygame.init()

WIDTH, HEIGHT = 800, 600
screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption("积木塔仿真")

WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
RED = (255, 0, 0)

LAYER_COLORS = [
    (255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (255, 0, 255),
    (0, 255, 255), (128, 0, 0), (0, 128, 0), (0, 0, 128), (128, 128, 0),
]

cap = cv2.VideoCapture(1)
cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G'))

cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)

class Block:
    def __init__(self, x, y, width, height, angle, layer):
        self.x = x
        self.y = y
        self.width = width
        self.height = height
        self.angle = angle
        self.layer = layer
        self.color = LAYER_COLORS[layer % len(LAYER_COLORS)]

    def draw(self, surface):
        rotated_surface = pygame.Surface((self.width, self.height), pygame.SRCALPHA)
        rotated_surface.set_alpha(125)
        pygame.draw.rect(rotated_surface, self.color, (0, 0, self.width, self.height))
        rotated_surface = pygame.transform.rotate(rotated_surface, self.angle)
        surface.blit(rotated_surface, (self.x - rotated_surface.get_width() // 2, self.y - rotated_surface.get_height() // 2))

    def get_corners(self):
        w, h = self.width / 2, self.height / 2
        corners = [(-w, -h), (w, -h), (w, h), (-w, h)]
        angle_rad = np.deg2rad(self.angle)
        rotation_matrix = np.array([
            [np.cos(angle_rad), -np.sin(angle_rad)],
            [np.sin(angle_rad), np.cos(angle_rad)]
        ])
        rotated_corners = np.dot(corners, rotation_matrix)
        translated_corners = rotated_corners + np.array([self.x, self.y])
        # print(translated_corners)
        rotated_corners = [(float(point[0]), float(point[1])) for point in translated_corners]

        return rotated_corners


class Tower:
    def __init__(self):
        self.blocks = []
        self.current_layer = 0
        self.layer_centroids = []  # 存储每层的重心
        self.stability_threshold = 10  # 稳定性阈值（像素）
        self.rotation_angle = 15  # 预设每层旋转角度
        self.angle_tolerance = 5  # 允许的角度偏移
        self.position_tolerance = 10  # 允许的位置偏移

        self.sim_to_robot_matrix = np.array([
            [1, 0, 0, -300],  # 假设的转换矩阵，需要根据实际情况调整
            [0, -1, 0, -200],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])
        self.robot_to_sim_matrix = np.linalg.inv(self.sim_to_robot_matrix)

        self.stack_bot = Stackbot()

        self.is_upper = False


    def sim_to_robot_coords(self, x, y, angle):
        sim_coords = np.array([x, y, 0, 1])
        robot_coords = self.sim_to_robot_matrix.dot(sim_coords)
        robot_angle = (angle + 360) % 360  # 确保角度在0-359范围内
        return robot_coords[0], robot_coords[1], robot_angle

    def robot_to_sim_coords(self, x, y, angle):
        robot_coords = np.array([x, y, 0, 1])
        sim_coords = self.robot_to_sim_matrix.dot(robot_coords)
        sim_angle = (angle + 360) % 360
        return sim_coords[0], sim_coords[1], sim_angle

    def add_block_from_robot(self, x, y, angle):
        """
        这个函数根据机器人的坐标和角度，在模拟世界中添加一个方块

        参数:
            x (float): 机器人的 x 坐标
            y (float): 机器人的 y 坐标
            angle (float): 机器人的角度

        返回值：
            bool：如果方块添加成功，返回 True，否则返回 False

        注意：这个函数需要先调用 robot_to_sim_coords 来将机器人坐标转换为模拟坐标，并且需要一个名为 Block 的类来创建方块对象
        """
        sim_x, sim_y, sim_angle = self.robot_to_sim_coords(x, y, angle)
        new_block = Block(sim_x, sim_y, 100, 20, sim_angle, self.current_layer)
        if self.add_block(new_block):
            print(f"Block added from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}")
            return True
        else:
            print(f"Failed to add block from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}")
            return False


    def add_block(self, block):
        if self.current_layer == 5:
            self.is_upper = True
            print("is upper")

        if not self.check_interference(block):
            self.blocks.append(block)
            self.stack_bot.pick_stack()
            robo_xyz = self.sim_to_robot_coords(block.x, block.y, block.angle)
            print(block.x, block.y, block.angle)
            if self.is_upper:
                self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer - 5)
            else:
                self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer)
            return True
        return False

    def draw(self, surface):
        for block in self.blocks:
            block.draw(surface)

    def check_stability(self):
        current_layer_centroid = self.calculate_current_layer_centroid()
        tower_centroid = self.calculate_tower_centroid()
        
        if current_layer_centroid is None or tower_centroid is None:
            return True  # 如果没有足够的块来计算重心，我们假设它是稳定的
        
        horizontal_distance = abs(current_layer_centroid[0] - tower_centroid[0])
        
        return horizontal_distance <= self.stability_threshold

    def blocks_overlap(self, block1, block2):
        corners1 = block1.get_corners()
        corners2 = block2.get_corners()
        
        # 检查 block1 的角是否在 block2 内部
        for corner in corners1:
            if self.point_inside_polygon(corner, corners2):
                return True
        
        # 检查 block2 的角是否在 block1 内部
        for corner in corners2:
            if self.point_inside_polygon(corner, corners1):
                return True
        
        # 检查边是否相交
        for i in range(4):
            for j in range(4):
                if self.line_intersects(corners1[i], corners1[(i+1)%4], corners2[j], corners2[(j+1)%4]):
                    return True
        
        return False

    def point_inside_polygon(self, point, polygon):
        x, y = point
        n = len(polygon)
        inside = False
        p1x, p1y = polygon[0]
        for i in range(n + 1):
            p2x, p2y = polygon[i % n]
            if y > min(p1y, p2y):
                if y <= max(p1y, p2y):
                    if x <= max(p1x, p2x):
                        if p1y != p2y:
                            xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
                        if p1x == p2x or x <= xinters:
                            inside = not inside
            p1x, p1y = p2x, p2y
        return inside

    def line_intersects(self, p1, p2, p3, p4):
        def ccw(A, B, C):
            return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0])
        return ccw(p1,p3,p4) != ccw(p2,p3,p4) and ccw(p1,p2,p3) != ccw(p1,p2,p4)

    def check_interference(self, new_block):
        # 检查是否与其他木块重叠
        for block in self.blocks:
            if block.layer == new_block.layer and self.blocks_overlap(block, new_block):
                return True
        return False

    def calculate_current_layer_centroid(self):
            current_layer_blocks = [block for block in self.blocks if block.layer == self.current_layer]
            if not current_layer_blocks:
                return None
            
            total_area = 0
            weighted_x = 0
            weighted_y = 0
            
            for block in current_layer_blocks:
                area = block.width * block.height
                total_area += area
                weighted_x += block.x * area
                weighted_y += block.y * area
            
            if total_area == 0:
                return None
            
            centroid_x = weighted_x / total_area
            centroid_y = weighted_y / total_area
            return (centroid_x, centroid_y)
    
    def calculate_tower_centroid(self):
        if not self.blocks:
            return None
        
        total_area = 0
        weighted_x = 0
        weighted_y = 0
        
        for block in self.blocks:
            area = block.width * block.height
            total_area += area
            weighted_x += block.x * area
            weighted_y += block.y * area
        
        if total_area == 0:
            return None
        
        centroid_x = weighted_x / total_area
        centroid_y = weighted_y / total_area
        return (centroid_x, centroid_y)

    def increase_layer(self):
        centroid = self.calculate_current_layer_centroid()
        if centroid:
            self.layer_centroids.append(centroid)
        self.current_layer += 1

    def auto_place_block(self):
        if self.current_layer == 0:
            # 对于第一层，直接在中心放置
            return Block(WIDTH // 2, HEIGHT // 2, 100, 20, 0, self.current_layer)

        base_centroid = self.layer_centroids[0] if self.layer_centroids else (WIDTH // 2, HEIGHT // 2)
        current_centroid = self.calculate_current_layer_centroid() or base_centroid

        # 计算当前层的总质量（假设质量与宽度成正比）
        total_mass = sum(block.width for block in self.blocks if block.layer == self.current_layer)
        
        # 假设新积木块的质量与宽度成正比
        new_block_mass = 100  # 新积木块的宽度

        # 计算理想位置
        ideal_x = (base_centroid[0] * (total_mass + new_block_mass) - current_centroid[0] * total_mass) / new_block_mass
        ideal_y = (base_centroid[1] * (total_mass + new_block_mass) - current_centroid[1] * total_mass) / new_block_mass

        # 计算木块中心到塔重心的角度
        dx = base_centroid[0] - ideal_x
        dy = base_centroid[1] - ideal_y
        center_to_centroid_angle = math.degrees(math.atan2(dy, dx))

        # 计算垂直于中心线的理想角度
        ideal_angle = (center_to_centroid_angle + 90) % 180 + 90

        # 步骤 2: 寻找最佳角度
        best_block = None
        min_angle_diff = float('inf')

        for angle in range(0, 180, 15):  # 每15度尝试一次
            test_block = Block(ideal_x, ideal_y, 100, 20, angle, self.current_layer)
            
            if not self.check_interference(test_block):
                # 检查是否有支撑
                if self.has_support(test_block):
                    # 计算与理想角度的差异
                    angle_diff = min((angle - ideal_angle) % 180, (ideal_angle - angle) % 180)
                    if angle_diff < min_angle_diff:
                        min_angle_diff = angle_diff
                        best_block = test_block

        return best_block

    def has_support(self, block):
        if self.current_layer == 0:
            return True  # 第一层总是有支撑

        block_corners = block.get_corners()
        for lower_block in [b for b in self.blocks if b.layer == self.current_layer - 1]:
            lower_corners = lower_block.get_corners()
            for corner in block_corners:
                if self.point_inside_polygon(corner, lower_corners):
                    return True
        return False
    
    def auto_place_layer(self):
        center_x, center_y = 359, 90
        block_width, block_height = 100, 20
        square_size = 110  # 正方形的边长

        # 基于当前层数计算旋转角度
        layer_rotation = (self.current_layer * 30) % 360
        
        # 计算正方形四个角的坐标
        base_positions = [
            (-square_size/2, -square_size/2),  # 左上
            (square_size/2, -square_size/2),   # 右上
            (square_size/2, square_size/2),    # 右下
            (-square_size/2, square_size/2)    # 左下
        ]

        # 旋转基础位置
        rotated_positions = []
        for x, y in base_positions:
            angle_rad = math.radians(layer_rotation)
            rotated_x = x * math.cos(angle_rad) - y * math.sin(angle_rad)
            rotated_y = x * math.sin(angle_rad) + y * math.cos(angle_rad)
            rotated_positions.append((center_x + rotated_x, center_y + rotated_y))

        base_angles = [45, 135, 225, 315]
        angles = [(angle - layer_rotation) % 360 for angle in base_angles]

        for (x, y), angle in zip(rotated_positions, angles):
            new_block = Block(x, y, block_width, block_height, angle, self.current_layer)
            if self.add_block(new_block):
                print(f"Block placed at ({x:.2f}, {y:.2f}) with angle {angle}")
            else:
                print(f"Failed to place block at ({x:.2f}, {y:.2f}) with angle {angle}")


    def clear_current_layer(self):
        self.blocks = [block for block in self.blocks if block.layer!= self.current_layer]

    def scan_current_layer(self):
        current_block_pos = cap_and_mark(cap=cap)
        for block in current_block_pos:
            print(block)
            self.add_block_from_robot(block[0][0], block[0][1], block[1])
            
            
def main():
    tower = Tower()
    clock = pygame.time.Clock()
    current_block = None
    
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.MOUSEBUTTONDOWN:
                if event.button == 1:
                    x, y = pygame.mouse.get_pos()
                    current_block = Block(x, y, 100, 20, 0, tower.current_layer)
            elif event.type == pygame.MOUSEMOTION:
                if current_block:
                    current_block.x, current_block.y = pygame.mouse.get_pos()
            elif event.type == pygame.MOUSEBUTTONUP:
                if event.button == 1 and current_block:
                    if tower.add_block(current_block):
                        current_block = None
                    else:
                        print("CANNOT place block here, there is already a block there.")
            elif event.type == pygame.KEYDOWN:
                # 处理所有按键事件
                if event.key == pygame.K_r and current_block:  # 按 'R' 键旋转当前块
                    current_block.angle += 15
                elif event.key == pygame.K_SPACE:  # 按 'Space' 键确认当前层完成放置
                    tower.increase_layer()
                elif event.key == pygame.K_a:  # 按 'A' 键自动放置
                    auto_block = tower.auto_place_block()
                    if auto_block:
                        tower.add_block(auto_block)
                    else:
                        print("Cannot find a suitable position for auto-placement.")
                elif event.key == pygame.K_n:  # 按 'N' 键自动放置新的一层
                    tower.auto_place_layer()
                elif event.key == pygame.K_s:  # 按 'S' 键扫描当前层
                    tower.scan_current_layer()


        screen.fill(WHITE)
        tower.draw(screen)
        if current_block:
            current_block.draw(screen)

        # 计算并绘制当前层的重心
        current_centroid = tower.calculate_current_layer_centroid()
        if current_centroid:
            pygame.draw.circle(screen, RED, (int(current_centroid[0]), int(current_centroid[1])), 5)
            font = pygame.font.Font(None, 24)

        # 绘制之前层的重心
        for i, centroid in enumerate(tower.layer_centroids):
            pygame.draw.circle(screen, BLACK, (int(centroid[0]), int(centroid[1])), 3)
            font = pygame.font.Font(None, 24)
            text = font.render(f"{i+1}", True, BLACK)
            screen.blit(text, (int(centroid[0]) + 10, int(centroid[1]) - 10))

        if not tower.check_stability():
            font = pygame.font.Font(None, 36)
            text = font.render("WARNING: Tower is NOT stable", True, RED)
            screen.blit(text, (WIDTH // 2 - text.get_width() // 2, 20))

        font = pygame.font.Font(None, 36)
        layer_text = font.render(f"Current layer: {tower.current_layer + 1}", True, BLACK)
        screen.blit(layer_text, (10, 10))

        pygame.display.flip()
        clock.tick(60)

    pygame.quit()

if __name__ == "__main__":
    main()