package org.firstinspires.ftc.teamcode.reorg;

import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.Telemetry;

public class AutonomousTestService implements AutonomousCommand {

    private DcMotor leftDrive;
    private DcMotor rightDrive;
    private DcMotor backRightDrive;
    private DcMotor backLeftDrive;
    private ColorSensor colorRight;
    private ColorSensor colorLeft;
    private Servo wrist;
    private Servo gripper;
    private DcMotor arm;
    private ElapsedTime runTime;

    public AutonomousTestService(HardwareMap hwMap) {
        this.runTime = new ElapsedTime();
        leftDrive = hwMap.get(DcMotor.class, "Drive front lt");
        rightDrive = hwMap.get(DcMotor.class, "Drive front rt");
        backLeftDrive = hwMap.get(DcMotor.class, "Drive back lt");
        backRightDrive = hwMap.get(DcMotor.class, "Drive back rt");
        colorRight = hwMap.get(ColorSensor.class, "color right");
        colorLeft = hwMap.get(ColorSensor.class, "color left");
        arm = hwMap.get(DcMotor.class, "arm raise");
        wrist = hwMap.get(Servo.class, "wrist");
        gripper = hwMap.get(Servo.class, "gripper");
    }

    public void initializeHardware() {
        setWristPosition(1);
        setGripperPosition(1);
        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    }

    private void setWristPosition(int position) {
        wrist.setPosition(position);
    }

    private void setGripperPosition(int position) {
        wrist.setPosition(position);
    }

    public void initializeDirection() {
        setDriveDirectionForward();
        setArmDirection(DcMotor.Direction.REVERSE);
    }

    public void initializeEncoders() {
        setDriveModeStopAndResetEncoder();
        setDriveModeRunUsingEncoder();
        setArmDriveModeStopAndResetEncoder();
        setArmDriveModeRunUsingEncoder();
    }

    private void setDriveDirectionForward() {
        leftDrive.setDirection(DcMotor.Direction.REVERSE);
        rightDrive.setDirection(DcMotor.Direction.FORWARD);
        backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
        backRightDrive.setDirection(DcMotor.Direction.REVERSE);
    }

    private void setDriveDirectionLeft() {
        leftDrive.setDirection(DcMotor.Direction.FORWARD);
        rightDrive.setDirection(DcMotor.Direction.FORWARD);
        backLeftDrive.setDirection(DcMotor.Direction.FORWARD);
        backRightDrive.setDirection(DcMotor.Direction.REVERSE);
    }

    private void setDriveDirectionStraightLeft() {
        leftDrive.setDirection(DcMotor.Direction.FORWARD);
        rightDrive.setDirection(DcMotor.Direction.FORWARD);
        backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
        backRightDrive.setDirection(DcMotor.Direction.FORWARD);
    }

    private void turn(double degrees, boolean isOpModeActive, Telemetry telemetry) {
        double turning_distance = degrees * AutonomousConstant.DEGREE_TOO_DISTANCE;
        move(turning_distance, isOpModeActive, telemetry);
    }

    private void move(double degrees, boolean isOpModeActive, Telemetry telemetry) {
        encoderDrive(AutonomousConstant.DRIVE_SPEED, degrees, degrees, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry);
    }

    private void setDriveMode(DcMotor.RunMode runMode) {
        leftDrive.setMode(runMode);
        rightDrive.setMode(runMode);
        backLeftDrive.setMode(runMode);
        backRightDrive.setMode(runMode);
    }
    private void setArmMode(DcMotor.RunMode runMode) {
        arm.setMode(runMode);
    }

    public void setArmDirection(DcMotor.Direction direction) {
        arm.setDirection(direction);
    }

    public void setDriveModeStopAndResetEncoder() {
        setDriveMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    }

    public void setDriveModeRunUsingEncoder() {
        setDriveMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }

    public void setArmDriveModeStopAndResetEncoder() {
        setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    }

    public void setArmDriveModeRunUsingEncoder() {
        setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }

    private void setDriveTargetPosition(int nlt, int nrt, int nbrt, int nblt) {
        leftDrive.setTargetPosition(nlt);
        rightDrive.setTargetPosition(nrt);
        backRightDrive.setTargetPosition(nbrt);
        backLeftDrive.setTargetPosition(nblt);
    }

    public void setDriveModeRunToPosition() {
        setDriveMode(DcMotor.RunMode.RUN_TO_POSITION);
    }

    public void setDrivePower(double speed) {
        leftDrive.setPower(Math.abs(speed));
        rightDrive.setPower(Math.abs(speed));
        backRightDrive.setPower(Math.abs(speed));
        backLeftDrive.setPower(Math.abs(speed));
    }

    public int getColorSensorReadingOnRightSide(Telemetry telemetry) {
        telemetry.addData("Clear Right", colorRight.alpha());
        telemetry.addData("Red Right", colorRight.red());
        telemetry.addData("Green Right", colorRight.green());
        telemetry.addData("Blue Right", colorRight.blue());
        telemetry.addData("Color Sensor","right");
        return colorRight.red();
    }

    public int getColorSensorReadingOnLeftSide(Telemetry telemetry) {
        telemetry.addData("Clear Left", colorLeft.alpha());
        telemetry.addData("Red Left ", colorLeft.red());
        telemetry.addData("Green Left", colorLeft.green());
        telemetry.addData("Blue Left", colorLeft.blue());
        return colorLeft.red();
    }

    private int calculateCurrentPosition(int position, double distance) {
        return position + (int) (distance * AutonomousConstant.COUNTS_PER_INCH);
    }

    private void drive(double distance, boolean isOpModeActive, Telemetry telemetry) {
        encoderDrive(AutonomousConstant.DRIVE_SPEED, distance, distance, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry);
    }

    private void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean isOpModeActive, Telemetry telemetry) {

        if (isOpModeActive) {

            int ldgcp = leftDrive.getCurrentPosition();
            int rdgcp = leftDrive.getCurrentPosition();
            int bldgcp = leftDrive.getCurrentPosition();
            int brdgcp = leftDrive.getCurrentPosition();

            int nlt = calculateCurrentPosition(ldgcp, leftInches);
            int nrt = calculateCurrentPosition(rdgcp, rightInches);
            int nblt = calculateCurrentPosition(bldgcp, leftInches);
            int nbrt = calculateCurrentPosition(brdgcp, rightInches);

            setDriveTargetPosition(nlt, nrt, nblt, nbrt);

            setDriveModeRunToPosition();

            runTime.reset();

            setDrivePower(speed);

            while (isOpModeActive && (runTime.seconds() < timeout) && (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) {
                telemetry.addData("Running to", " %7d :%7d", nlt, nrt);
                telemetry.addData("Currently at", " at %7d :%7d", ldgcp, rdgcp, bldgcp, brdgcp);
                telemetry.update();
            }

            setDrivePower(0);

            setDriveModeRunUsingEncoder();

        }

    }

    @Override
    public void execute(boolean isOpModeActive, Telemetry telemetry) {
        /*
        Do we need this?  Can it be part of the initialization?
         */
        // arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        /*
        Drive forward to left/right/center placement of item
         */
        setDriveDirectionForward();
        drive(26, isOpModeActive, telemetry);



        if (getColorSensorReadingOnLeftSide(telemetry) > 50) {

            if(getColorSensorReadingOnLeftSide(telemetry) > getColorSensorReadingOnRightSide(telemetry))
                telemetry.addData("color sensor","left");

            setDriveDirectionLeft();
            turn(90, isOpModeActive, telemetry);

            setDriveDirectionStraightLeft();
            move(8.5, isOpModeActive, telemetry);

        }


        if (getColorSensorReadingOnRightSide(telemetry) > 50) {

            if(getColorSensorReadingOnRightSide(telemetry) > getColorSensorReadingOnLeftSide(telemetry))
                telemetry.addData("color sensor","right");

        }


    }
}
