package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.Set;

public class TestHardware {
    private LinearOpMode myOpMode;   // access to the calling OpMode

    // Discovered motors
    public DcMotor[] motors = new DcMotor[0];
    public String[] motorNames = new String[0];

    public int motorIndex = -1; // current selected motor index

    public TestHardware (LinearOpMode opmode) {
        myOpMode = opmode;
    }

    public void init() {
        // Discover all DcMotor names
        Set<String> nameSet = myOpMode.hardwareMap.keySet(DcMotor.class);
        List<String> names = new ArrayList<>(nameSet);
        // Sort for stable order across runs
        Collections.sort(names);

        List<DcMotor> motorList = new ArrayList<>(names.size());
        for (String name : names) {
            DcMotor m = myOpMode.hardwareMap.get(DcMotor.class, name);
            motorList.add(m);
        }
        motors = motorList.toArray(new DcMotor[0]);
        motorNames = names.toArray(new String[0]);
        motorIndex = motors.length > 0 ? 0 : -1;

        // Telemetry summary
        myOpMode.telemetry.addData(">", "Dynamic Hardware Initialized");
        myOpMode.telemetry.addData("total motors", motors.length);
        if (motors.length > 0) {
            myOpMode.telemetry.addData("motor names", String.join(", ", motorNames));
        }
        myOpMode.telemetry.update();
    }

    // Cycle forward through discovered motors
    public void cycleMotorPos() {
        if (motorIndex < 0 || motors.length == 0) return;
        motorIndex = (motorIndex + 1) % motors.length;
    }

    // Cycle backward through discovered motors
    public void cycleMotorNeg() {
        if (motorIndex < 0 || motors.length == 0) return;
        motorIndex = (motorIndex - 1 + motors.length) % motors.length;
    }

    public String getCurrentMotorName() {
        return (motorIndex >= 0 && motorIndex < motorNames.length) ? motorNames[motorIndex] : "";
    }

    public DcMotor getCurrentMotor() {
        return (motorIndex >= 0 && motorIndex < motors.length) ? motors[motorIndex] : null;
    }

    public void motorPowerPos() {
        DcMotor m = getCurrentMotor();
        if (m != null) m.setPower(1.0);
    }

    public void motorPowerNeg() {
        DcMotor m = getCurrentMotor();
        if (m != null) m.setPower(-1.0);
    }

    public void motorPowerStop() {
        DcMotor m = getCurrentMotor();
        if (m != null) m.setPower(0.0);
    }


    // Update telemetry about current selection
    public void reportSelection() {
        myOpMode.telemetry.addData("selected index", motorIndex);
        myOpMode.telemetry.addData("selected name", getCurrentMotorName());
        DcMotor m = getCurrentMotor();
        if (m != null) {
            myOpMode.telemetry.addData("selected power", m.getPower());
        }
        myOpMode.telemetry.update();
    }

    public void sleep(int millis) {
        try {
            Thread.sleep(millis);
        } catch (InterruptedException e) {
            Thread.currentThread().interrupt();
        }
    }
}