#!/usr/bin/env python3
"""
Servidor HTTP para generar y servir mapas SLAM + paredes a Unity
"""
from flask import Flask, jsonify
from flask_cors import CORS
import subprocess
import os
import time
import base64
import json
from pathlib import Path

app = Flask(__name__)
CORS(app)

# Configuración
MAPS_DIR = "/home/iberomsc02/maps"
MAP_NAME = "current_map"
EXTRACT_WALLS_SCRIPT = "/home/iberomsc02/extract_walls.py"

@app.route('/health', methods=['GET'])
def health():
    """Verificar que el servidor está activo"""
    return jsonify({
        "status": "ok",
        "message": "SLAM Server running",
        "maps_dir": MAPS_DIR
    })

@app.route('/generate_map', methods=['GET'])
def generate_map():
    """
    Genera mapa con ROS2, extrae paredes y devuelve todo
    """
    try:
        print(f"🗺️ Generando mapa: {MAP_NAME}")
        
        # Crear directorio si no existe
        os.makedirs(MAPS_DIR, exist_ok=True)
        
        # 1) Ejecutar ROS2 map_saver
        ros2_cmd = f"ros2 run nav2_map_server map_saver_cli -f {MAPS_DIR}/{MAP_NAME}"
        result = subprocess.run(
            ros2_cmd,
            shell=True,
            capture_output=True,
            text=True,
            timeout=30
        )
        
        if result.returncode != 0:
            return jsonify({
                "status": "error",
                "message": f"ROS2 command failed: {result.stderr}"
            }), 500
        
        time.sleep(2)  # Esperar a que se escriban los archivos
        
        pgm_path = f"{MAPS_DIR}/{MAP_NAME}.pgm"
        yaml_path = f"{MAPS_DIR}/{MAP_NAME}.yaml"
        walls_json_path = f"{MAPS_DIR}/{MAP_NAME}_walls.json"
        
        # Verificar archivos ROS2
        if not os.path.exists(pgm_path) or not os.path.exists(yaml_path):
            return jsonify({
                "status": "error",
                "message": "Map files not generated by ROS2"
            }), 500
        
        # 2) Ejecutar extract_walls.py
        print("🧱 Extrayendo paredes...")
        walls_cmd = f"python3 {EXTRACT_WALLS_SCRIPT} {yaml_path} {walls_json_path}"
        
        walls_result = subprocess.run(
            walls_cmd,
            shell=True,
            capture_output=True,
            text=True,
            timeout=30
        )
        
        if walls_result.returncode != 0:
            print(f"⚠️ Warning: Wall extraction failed: {walls_result.stderr}")
            # Continuar sin paredes (devolver JSON vacío)
            walls_data = {"version": 3, "wall_segments": [], "corners": []}
            walls_json = json.dumps(walls_data)
        else:
            # Leer JSON de paredes
            with open(walls_json_path, 'r') as f:
                walls_json = f.read()
        
        # 3) Leer archivos
        with open(pgm_path, 'rb') as f:
            pgm_data = f.read()
        
        with open(yaml_path, 'r') as f:
            yaml_data = f.read()
        
        print(f"✅ Archivos generados:")
        print(f"   PGM: {len(pgm_data)} bytes")
        print(f"   YAML: {len(yaml_data)} bytes")
        print(f"   Walls JSON: {len(walls_json)} bytes")
        
        # 4) Devolver todo en JSON
        return jsonify({
            "status": "success",
            "map_name": MAP_NAME,
            "pgm": base64.b64encode(pgm_data).decode('utf-8'),
            "yaml": yaml_data,
            "walls": walls_json,  # ✨ NUEVO: JSON de paredes
            "pgm_size": len(pgm_data),
            "yaml_size": len(yaml_data),
            "walls_size": len(walls_json)
        })
        
    except subprocess.TimeoutExpired:
        return jsonify({
            "status": "error",
            "message": "Command timeout"
        }), 500
    except Exception as e:
        return jsonify({
            "status": "error",
            "message": str(e)
        }), 500

if __name__ == '__main__':
    print("=" * 60)
    print("🚀 SLAM Server iniciando...")
    print(f"📁 Directorio de mapas: {MAPS_DIR}")
    print(f"🧱 Script de paredes: {EXTRACT_WALLS_SCRIPT}")
    print(f"🌐 Servidor en: http://0.0.0.0:5008")
    print("=" * 60)
    
    app.run(host='0.0.0.0', port=5008, debug=False, threaded=True)