package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ReadWriteFile;

import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;

import java.io.File;
import java.util.UUID;

public class API {
    private OpMode opMode;
    public HubIMU imu;
    public boolean useLogFile = false;
    public File logFile = null;
    public Gamepad gamepad1;
    public Gamepad gamepad2;

    /**
     * @param mode the opmode to initialize with
     * @param useLogFile whether or not to log to a log file
     */
    public API(final OpMode mode, boolean useLogFile) {
        opMode = mode;

        print("Initializing API for class " + opMode.getClass().getName() + "...");

//        for (Motor m : Motor.values()) m.init(mode.hardwareMap);
//        for (Servo s : Servo.values()) s.init(mode.hardwareMap);
        imu = new HubIMU("imu");
        gamepad1 = new Gamepad(GamepadIndex.GAMEPAD1);
        gamepad2 = new Gamepad(GamepadIndex.GAMEPAD2);

        this.useLogFile = useLogFile;
        if (useLogFile)
            logFile = AppUtil.getInstance().getSettingsFile("log-" + UUID.randomUUID().toString() + ".log");
    }

    /**
     * @param mode the opmode to initialize with
     */
    public API(final OpMode mode) {
        this(mode, true);
    }

    /**
     * Pauses for a given amount of seconds, with sub-millisecond accuracy
     *
     * @param seconds the amount of seconds to pause for
     */
    public void pause(double seconds) {
        double time = opMode.getRuntime() + seconds;
        while (opMode.getRuntime() < time) ; /* no empty while loop here! */
    }

    /**
     * Prints a line to telemetry
     *
     * @param s the text to print
     * @see API#print(String, String)
     */
    public void print(String s) {
        opMode.telemetry.addLine(s);
        opMode.telemetry.update();

        if (useLogFile)
            ReadWriteFile.writeFile(logFile, ReadWriteFile.readFile(logFile) + "\n\n" + s);
    }

    /**
     * Prints a value to telemetry, formatted as "caption: value"
     *
     * @param caption the caption to print
     * @param value   the value to print
     * @see API#print(String)
     */
    public void print(String caption, String value) {
        opMode.telemetry.addData(caption, value);
        opMode.telemetry.update();

        if (useLogFile)
            ReadWriteFile.writeFile(logFile, ReadWriteFile.readFile(logFile) + "\n\n" + caption + ": " + value);
    }

    /**
     * Clears the telemetry
     */
    public void clear() {
        opMode.telemetry.clear();
    }

    public class Motor {
//        M0("m0"), M1("m1"), M2("m2"), M3("m3"), M4("m4"), M5("m5"), M6("m6"), M7("m7");

        private final String name;
        private DcMotor rawMotor;
        private Direction direction = Direction.FORWARD;
        private double power = 0;

        Motor(String name) {
            this.name = name;
            rawMotor = opMode.hardwareMap.get(DcMotor.class, name);
            rawMotor.setPower(0);
        }

        /**
         * Starts the motor with the previously set power
         */
        public void start() {
            rawMotor.setPower(power * direction.i);
        }

        /**
         * Starts the motor with the specified power
         *
         * @param power the power to use for the motor
         */
        public void start(double power) {
            setPower(power);
            start();
        }

        /**
         * Stops the motor
         */
        public void stop() {
            rawMotor.setPower(0);
        }

        /**
         * Sets the power without starting the motor
         *
         * @param power the power to use for the motor
         */
        public void setPower(double power) {
            this.power = power;
        }

        /**
         * Gets the previously set power
         *
         * @return the power
         */
        public double getPower() {
            return this.power;
        }

        /**
         * Gets the power the motor is currently moving at, or 0 if not moving
         *
         * @return the power
         */
        public double getRawPower() {
            return this.rawMotor.getPower();
        }

        /**
         * Sets the direction the motor should move in, without starting the motor
         *
         * @param direction the direction to use for the motor
         */
        public void setDirection(Direction direction) {
            setDirection(direction, false);
        }

        /**
         * Sets the direction the motor should move in
         *
         * @param direction the direction to use for the motor
         * @param immediate whether to immediately start the motor with the new direction
         */
        public void setDirection(Direction direction, boolean immediate) {
            this.direction = direction;
            if (immediate) start();
        }

        /**
         * Stops and resets the encoder
         */
        public void resetEncoder() {
            rawMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        }

        /**
         * Set the zero-power behaviour of the motor.
         *
         * BRAKE means to immediately apply brakes when power is 0, and FLOAT means to come to a rolling stop.
         *
         * @param behaviour the behaviour to use
         */
        public void setBehaviour(MotorBehaviour behaviour) {
            rawMotor.setZeroPowerBehavior(
                    behaviour == MotorBehaviour.BRAKE ?
                            DcMotor.ZeroPowerBehavior.BRAKE :
                            DcMotor.ZeroPowerBehavior.FLOAT
            );
        }

        /**
         * Moves the motor forward or backward at the provided speed based on the button inputs
         *
         * @param positiveControl the button used to move the motor forward
         * @param negativeControl the button used to move the motor backward
         */
        public void controlWithTwoButtons(boolean positiveControl, boolean negativeControl, double speed) {
            if (positiveControl && !negativeControl) this.start(speed);
            else if (negativeControl && !positiveControl) this.start(-speed);
            else this.stop();
        }

        /**
         * Moves the motor forward or backward at maximum speed based on the button inputs
         *
         * @param positiveControl the button used to move the motor forward
         * @param negativeControl the button used to move the motor backward
         */
        public void controlWithTwoButtons(boolean positiveControl, boolean negativeControl) {
            this.controlWithTwoButtons(positiveControl, negativeControl, 1);
        }

        /**
         * Moves the motor forward at the provided speed if the button is pressed.
         *
         * @param positiveControl the button used to move forward
         */
        public void controlWithOneButton(boolean positiveControl, double speed) {
            this.controlWithTwoButtons(positiveControl, false, speed);
        }

        /**
         * Moves the motor forward at maximum speed if the button is pressed.
         *
         * @param positiveControl the button used to move forward
         */
        public void controlWithOneButton(boolean positiveControl) {
            this.controlWithOneButton(positiveControl, 1);
        }
    }

    public enum MotorBehaviour {
        BRAKE,
        FLOAT
    }

    public enum Direction {
        FORWARD(1), REVERSE(-1);
        private final int i;

        Direction(int i) {
            this.i = i;
        }
    }

    public class Servo {
//        S0("s0"), S1("s1"), S2("s2"), S3("s3"), S4("s4"), S5("s5"), S6("s6"), S7("s7"), S8("s8"), S9("s9"), S10("s10"), S11("s11");
        private final String name;
        private com.qualcomm.robotcore.hardware.Servo servo;
        private Direction direction = Direction.FORWARD;
        private double power = 0;

        Servo(String name) {
            this.name = name;
            servo = opMode.hardwareMap.get(com.qualcomm.robotcore.hardware.Servo.class, name);
        }

        /**
         * Sets the intended position of the servo, in degrees.
         * Note that intended means it will not necessarily get to this position, but that it will constantly attempt to get there.
         *
         * @param degrees The intended position.
         */
        public void setPosition(double degrees) {
            servo.setPosition(degrees);
        }

        /**
         * Gets the current position of the servo. Might not match up with setPosition.
         *
         * @return The position in degrees.
         */
        public double getPosition() {
            return servo.getPosition();
        }

        /**
         * Starts the servo
         *
         * @param power The power to use, from 0 to 1
         */
        public void start(double power) {
            this.power = power;
            power *= direction.i;
            servo.setPosition(power / 2 + 0.5);
        }

        /**
         * Stops the servo by setting the power to 0.
         */
        public void stop() {
            start(0);
        }

        /**
         * Sets the direction of the servo.
         *
         * @param direction The direction.
         * @param immediate Whether to start the servo upon setting direction or not.
         */
        public void setDirection(Direction direction, boolean immediate) {
            this.direction = direction;
            if (immediate) {
                start(power);
            }
        }
    }

    public class HubIMU {
        private final BNO055IMU imu;
        private double zeroPos;

        public HubIMU(String name) {
            imu = opMode.hardwareMap.get(BNO055IMU.class, name);
            setParameters();
        }

        private void setParameters() {
            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
            parameters.useExternalCrystal = true;
            parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
            parameters.pitchMode = BNO055IMU.PitchMode.WINDOWS;
            parameters.loggingEnabled = true;
            parameters.loggingTag = "IMU";
            imu.initialize(parameters);
        }

        private double[] getAngles() {
            Quaternion quatAngles = imu.getQuaternionOrientation();
            double w = quatAngles.w;
            double x = quatAngles.x;
            double y = quatAngles.y;
            double z = quatAngles.z;

            double roll = Math.toDegrees(Math.atan2(2 * (w*x + y*z), 1 - 2 * (x*x + y*y)));
            double pitch = Math.toDegrees(Math.asin(2 * (w*y - x*z)));
            double yaw = Math.toDegrees(Math.atan2(2 * (w*z + x*y), 1 - 2 * (y*y + z*z)));

            return new double[]{yaw, pitch, roll};
        }

        /**
         * Adjusts an angle so that -180 &#60;= angle &#60; 180.
         *
         * @param angle the angle to adjust
         * @return the adjusted angle
         */
        public double adjustAngle(double angle) {
            return ((angle + 180) % 360) - 180;
        }

        /**
         * Gets the yaw change since the last time the IMU was reset
         *
         * @return the yaw change
         */
        public double getHeading() {
            return getYaw() - zeroPos;
        }

        /**
         * Gets the yaw
         *
         * @return the yaw
         */
        public double getYaw() {
            return getAngles()[0];
        }

        /**
         * Resets the IMU
         */
        public void reset() {
            zeroPos = getYaw();
        }

        /**
         * Gets the pitch
         *
         * @return the pitch
         */
        public double getPitch() {
            return getAngles()[1];
        }

        /**
         * Gets the roll
         *
         * @return the roll
         */
        public double getRoll() {
            return getAngles()[2];
        }
    }

    public enum GamepadIndex {
        GAMEPAD1, GAMEPAD2
    }

    public class Gamepad {
        private com.qualcomm.robotcore.hardware.Gamepad gamepad;

        Gamepad(GamepadIndex index) {
            this.gamepad = index == GamepadIndex.GAMEPAD1 ? opMode.gamepad1 : opMode.gamepad2;
        }

        // Sticks
        public double getLeftStickX() {
            return this.gamepad.left_stick_x;
        }

        public double getLeftStickY() {
            return this.gamepad.left_stick_y;
        }

        public double getRightStickX() {
            return this.gamepad.right_stick_x;
        }

        public double getRightStickY() {
            return this.gamepad.right_stick_y;
        }

        // ABXY
        public boolean getA() {
            return this.gamepad.a;
        }

        public boolean getB() {
            return this.gamepad.b;
        }

        public boolean getX() {
            return this.gamepad.x;
        }

        public boolean getY() {
            return this.gamepad.y;
        }

        public int getABXY() {
            return
                    0b1000 * (this.getA() ? 1 : 0) +
                    0b0100 * (this.getB() ? 1 : 0) +
                    0b0010 * (this.getX() ? 1 : 0) +
                    0b0001 * (this.getY() ? 1 : 0);
        }

        // D-Pad
        public boolean getDPadUp() {
            return this.gamepad.dpad_up;
        }

        public boolean getDPadDown() {
            return this.gamepad.dpad_down;
        }

        public boolean getDPadLeft() {
            return this.gamepad.dpad_left;
        }

        public boolean getDPadRight() {
            return this.gamepad.dpad_right;
        }

        public int getDPad() {
            return
                    0b1000 * (this.getDPadUp() ? 1 : 0) +
                    0b0100 * (this.getDPadDown() ? 1 : 0) +
                    0b0010 * (this.getDPadLeft() ? 1 : 0) +
                    0b0001 * (this.getDPadRight() ? 1 : 0);
        }

        // Bumpers
        public boolean getLB() {
            return this.gamepad.left_bumper;
        }

        public boolean getRB() {
            return this.gamepad.right_bumper;
        }

        public int getBumpers() {
            return
                    0b10 * (this.getLB() ? 1 : 0) +
                    0b01 * (this.getRB() ? 1 : 0);
        }

        // Triggers
        public double getLT() {
            return this.gamepad.left_trigger;
        }

        public double getRT() {
            return this.gamepad.right_trigger;
        }

        // Stick clicks
        public boolean getL3() {
            return this.gamepad.left_stick_button;
        }

        public boolean getR3() {
            return this.gamepad.right_stick_button;
        }

        public double getStickClicks() {
            return
                    0b10 * (this.getL3() ? 1 : 0) +
                    0b01 * (this.getR3() ? 1 : 0);
        }

        // Misc
        public boolean getGuide() {
            return this.gamepad.guide;
        }

        public boolean getStart() {
            return this.gamepad.start;
        }

        public boolean getBack() {
            return this.gamepad.back;
        }

        public int getMiscButtons() {
            return
                    0b100 * (this.getGuide() ? 1 : 0) +
                    0b010 * (this.getStart() ? 1 : 0) +
                    0b001 * (this.getBack() ? 1 : 0);
        }

        public double getAllButtons() {
            return
                    0b000000000000001 * this.getABXY() +
                    0b000000000010000 * this.getDPad() +
                    0b000000100000000 * this.getBumpers() +
                    0b000010000000000 * this.getStickClicks() +
                    0b001000000000000 * this.getMiscButtons();
        }
    }
}