from flask import Blueprint, Flask, jsonify, render_template, request
import atexit

"""
Flask 控制台（Rosmaster_Lib）——麦克纳姆轮小车
使用 set_car_motion(vx, vy, vz) 控制：
  vx>0 前进，vx<0 后退；vy>0 左移，vy<0 右移；vz 为角速度（此处固定 0）。
"""


class RosmasterWrapper:
    def __init__(self):
        self.ok = False
        self.err = None
        try:
            from Rosmaster_Lib import Rosmaster
            self.board = Rosmaster()  # 使用出厂 /dev/myserial
            try:
                self.board.create_receive_threading()
            except Exception:
                pass
            self.ok = True
        except Exception as e:
            self.err = str(e)
            self.board = None

    def motion(self, vx=0.0, vy=0.0, vz=0.0):
        if not self.ok:
            raise RuntimeError(f"Rosmaster init failed: {self.err}")
        self.board.set_car_motion(vx, vy, vz)

    def stop(self):
        if self.ok:
            try:
                self.board.set_car_motion(0.0, 0.0, 0.0)
            except Exception:
                pass

    def cleanup(self):
        self.stop()


drv = RosmasterWrapper()


def speed01(pct) -> float:
    try:
        s = max(0, min(100, int(pct)))
    except Exception:
        s = 70
    return round(s / 100.0, 3)


driver_bp = Blueprint("driver", __name__, template_folder="templates")


@driver_bp.route('/')
def index():
    return render_template('index.html')


@driver_bp.post('/move')
def move():
    data = request.get_json(silent=True) or {}
    d = (data.get('dir') or '').lower()
    sp = speed01(data.get('speed', 70))
    try:
        if d == 'forward':
            drv.motion(vx=sp, vy=0.0, vz=0.0)
        elif d == 'back':
            drv.motion(vx=-sp, vy=0.0, vz=0.0)
        elif d == 'left':
            drv.motion(vx=0.0, vy=sp, vz=0.0)
        elif d == 'right':
            drv.motion(vx=0.0, vy=-sp, vz=0.0)
        elif d == 'stop':
            drv.stop()
        else:
            return jsonify(ok=False, error='unknown direction'), 400
        return jsonify(ok=True, dir=d, speed=int(data.get('speed', 70)))
    except Exception as e:
        return jsonify(ok=False, error=str(e)), 500


@driver_bp.get('/health')
def health():
    return jsonify(ok=True, rosmaster_ready=drv.ok, error=drv.err if not drv.ok else None)


@atexit.register
def _on_exit():
    drv.cleanup()


def create_app():
    app = Flask(__name__)
    app.register_blueprint(driver_bp)
    return app


if __name__ == '__main__':
    create_app().run(host='0.0.0.0', port=5000, debug=True)
