import cv2
import sys
from config import Config
from skeleton.mediapipe_detector import MediaPipeDetector
from gesture_control.arm_control import ArmController
from gesture_control.special_gestures import SpecialGestureDetector
from gesture_control.state import ControlState
from robot.map_simulator import MapSimulator
from robot.dummy import DummyRobot

def main():
    cfg = Config()

    # Детектор
    detector = MediaPipeDetector()

    # Состояние управления
    state = ControlState()

    # Специальные жесты
    special_detector = SpecialGestureDetector(
        mode=cfg.SPECIAL_GESTURE_MODE,
        model_path=cfg.ML_GESTURE_MODEL,
        class_names=cfg.ML_GESTURE_CLASSES
    )
    
    # Динамические жесты
    if cfg.DYNAMIC_GESTURE['enabled']:
        from ml_gestures_dynamic.predict import DynamicGesturePredictor
        dynamic_predictor = DynamicGesturePredictor(
            cfg.DYNAMIC_GESTURE['model_path'],
            cfg.DYNAMIC_GESTURE['classes_path'],
            cfg.DYNAMIC_GESTURE['window_size'],
            cfg.DYNAMIC_GESTURE['threshold']
        )
        
    # Управление скоростями
    arm_control = ArmController(cfg.ARM_CONTROL, mirror=cfg.MIRROR_CAMERA)

    # Робот
    if cfg.ROBOT_MODE == 'simulator':
        robot = MapSimulator(cfg)
    else:
        robot = DummyRobot()

    # Камера
    cap = cv2.VideoCapture(cfg.CAMERA_ID)
    if not cap.isOpened():
        print("Ошибка: не удалось открыть камеру")
        sys.exit(1)

    print("Управление: крест руками = СТОП, домик = ПУСК")
    print("Линейная скорость: правая рука в сторону, угловая: левая рука")
    print("В симуляторе: R – перезапуск после Game Over/победы")
    print("q – выход")

    while True:
        ret, frame = cap.read()
        if not ret:
            break

        if cfg.MIRROR_CAMERA:
            frame = cv2.flip(frame, 1)

        result = detector.detect(frame)

        if result['success']:
            landmarks = result['landmarks']
            
            if result['success']:
                if cfg.DYNAMIC_GESTURE['enabled']:
                    dynamic_predictor.add_frame(landmarks)
                    gesture = dynamic_predictor.predict()
                    if gesture:
                        action = cfg.DYNAMIC_GESTURE['actions'].get(gesture)
                        if action == 'reset':
                            robot.reset()
                        elif action == 'restart':
                            robot.reset()
                
            vis_frame = detector.draw_landmarks(frame, result['pose_landmarks'])

            special = special_detector.predict(landmarks)
            state.update(special)

            if state.enabled:
                linear, angular = arm_control.compute_speeds(landmarks)
            else:
                linear, angular = 0.0, 0.0

            robot.set_speeds(linear, angular)

            # Отрисовка на видео
            cv2.putText(vis_frame, f"State: {'ON' if state.enabled else 'OFF'}",
                        (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1,
                        (0,255,0) if state.enabled else (0,0,255), 2)
            cv2.putText(vis_frame, f"L:{linear:.2f} A:{angular:.2f}",
                        (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
            if special != 'none':
                cv2.putText(vis_frame, f"Special: {special}", (10, 90),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
        else:
            vis_frame = frame
            cv2.putText(vis_frame, "No pose detected", (10, 30),
                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)

        cv2.imshow('Camera', vis_frame)

        if cfg.ROBOT_MODE == 'simulator':
            if not robot.step():
                break
        else:
            robot.step()

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()
    robot.quit()

if __name__ == '__main__':
    main()