from flask import Flask, request, jsonify
import sys
import os

sys.path.insert(0, os.path.join(os.path.dirname(__file__), 'Rasp'))

import time
import pigpio
import motion
from motion import (
    motion_setup, motion_setup_steppers, motion_setup_sensors, motion_cleanup,
    enable_wheels, enable_arms, enable_syringe,
    arms_home, syringe_home,
    distance_to_object,
    ON, OFF, CW, CCW,
)

def _pi_connect():
    pi = pigpio.pi()
    if not pi.connected:
        print("ERROR: pigpiod no està en marxa. Executa: sudo pigpiod -s 1")
        sys.exit(1)
    return pi

def _setup_motors():
    """Setup mínim per a tests de motors (sense sensors I2C)."""
    pi = _pi_connect()
    motion_setup_steppers(pi)
    return pi

def _setup_sensors():
    """Setup mínim per a tests de sensors I2C (sense steppers)."""
    pi = _pi_connect()
    motion_setup_sensors(pi)
    return pi

def _teardown_motors(pi):
    motion_cleanup()
    pi.stop()

def _teardown_sensors(pi):
    pi.stop()

def test_arms_pair():
    pi = _setup_motors()
    enable_arms(ON); time.sleep(0.1)
    motion.arm_R.move(+200); motion.arm_L.move(+200)
    time.sleep(1.5)
    motion.arm_R.move(-200); motion.arm_L.move(-200)
    time.sleep(0.5)
    enable_arms(OFF)

app = Flask(__name__)

@app.route("/")
def home():
    return jsonify({
        "message": "Flask API is running"
    })

@app.route("/greet", methods=["GET"])
def greet():
    test_arms_pair()
    return jsonify({
        "message": "Hello, world!"
    })


# Simple error handlers
@app.errorhandler(404)
def not_found(e):
    return jsonify({"error": "Route not found"}), 404


@app.errorhandler(500)
def server_error(e):
    return jsonify({"error": "Internal server error"}), 500


if __name__ == "__main__":
    app.run(debug=True, host="0.0.0.0", port=8000)