package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

import java.util.List;

public class Hardware {
    public final OpMode opMode;
    public final DashboardTelemetryWrapper dashboardTelemetry;
    public final DcMotor fl;
    public final DcMotor fr;
    public final DcMotor bl;
    public final DcMotor br;
    public final DcMotor spool;
    public final Servo clawLeft;
    public final Servo clawRight;
    public final Servo wrist;
    public final IMU imu;
    public final List<LynxModule> hubs;

    public Hardware(OpMode opMode) {
        HardwareMap hm = opMode.hardwareMap;
        this.opMode = opMode;
        this.dashboardTelemetry = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
        opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetry);

        this.fl = hm.get(DcMotor.class, "fl");
        this.fr = hm.get(DcMotor.class, "fr");
        this.bl = hm.get(DcMotor.class, "bl");
        this.br = hm.get(DcMotor.class, "br");

        this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
        this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
        this.fr.setDirection(DcMotorSimple.Direction.FORWARD);
        this.br.setDirection(DcMotorSimple.Direction.FORWARD);
        this.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        this.fl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        this.fr.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        this.bl.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        this.br.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        this.spool = hm.get(DcMotor.class, "spool");
        this.clawLeft = hm.get(Servo.class, "clawLeft");
        this.clawRight = hm.get(Servo.class, "clawRight");
        this.wrist = hm.get(Servo.class, "wrist");
        this.imu = hm.get(IMU.class, "imu");
//        this.posImu = hm.get(BNO055IMU.class, "imu");

        this.spool.setDirection(DcMotorSimple.Direction.FORWARD);
        this.spool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        this.clawLeft.scaleRange(0, 1/3.);
        this.clawRight.scaleRange(2/3., 1);
        this.wrist.scaleRange(0.25, 1);

        this.hubs = hm.getAll(LynxModule.class);
        for (LynxModule hub : this.hubs) {
            hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
        }
    }

    public void setClawPosition(double pos) {
        this.clawLeft.setPosition(1 - pos);
        this.clawRight.setPosition(pos);
    }

    public void setWristPosition(double pos) {
        this.wrist.setPosition((pos + 1) / 2);
    }

    public void initIMU() {
        IMU.Parameters parameters = new IMU.Parameters(
                new RevHubOrientationOnRobot(
                        RevHubOrientationOnRobot.LogoFacingDirection.FORWARD,
                        RevHubOrientationOnRobot.UsbFacingDirection.LEFT
                )
        );

        this.imu.initialize(parameters);
    }

//    public void initPosIMU(BNO055IMU.AccelerationIntegrator integrator) {
//        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
//        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
//        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
//        parameters.calibrationDataFile = "AdafruitIMUCalibration.json";
//        parameters.accelerationIntegrationAlgorithm = integrator;
//
//        this.posImu.initialize(parameters);
//        this.posImu.stopAccelerationIntegration();
//    }

    public double getImuYaw() {
        return -this.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
    }

    public int getLiftEncoder() {
        return -spool.getCurrentPosition();
    }

//    public FieldVector getIMUPosition() {
//        Position tmp = this.posImu.getPosition().toUnit(DistanceUnit.MM);
//        return FieldVector.inMM(tmp.x, tmp.y);
//    }
}