package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.Hardware;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.hardware.CRServoImplEx;
import com.qualcomm.robotcore.hardware.CRServoImpl;
import com.qualcomm.robotcore.hardware.PwmControl



@TeleOp(name="MecanumDriveCode2", group="Mecanum")  // @Autonomous(...) is the other common choice

public class ExampleMecanumWheelCode2 extends OpMode
{
    /* Declare OpMode members. */



    CRServo leftdrive;
    CRServo Leftback;
    CRServo reallyrightback;
    CRServo rightdrive;





    /*
     * Code to run ONCE when the driver hits INIT
     */
    @Override
    public void init() {

        /* eg: Initialize the hardware variables. Note that the strings used here as parameters
         * to 'get' must correspond to the names assigned during the robot configuration
         * step (using the FTC Robot Controller app on the phone).
         */
        // leftMotor  = hardwareMap.dcMotor.get("left motor");
        // rightMotor = hardwareMap.dcMotor.get("right motor");


        // eg: Set the drive motor directions:
        // Reverse the motor that runs backwards when connected directly to the batteryYou mat need to tune these based on how your motors are mounted.


        telemetry.addData("Status", "Initialized");



    }

    /*
     * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
     */
    @Override
    public void init_loop() {
    }

    /*
     * Code to run ONCE when the driver hits PLAY
     */


    /*
     * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
     */
    @Override
    public void loop() {

        // Sets Motors in Brake Mode which prevents the robot from being easily pushed while stationary


        // Sets joysticks as variables to simply equation
        float Ch1 = -(gamepad1.right_stick_x);
        float Ch3 = -(gamepad1.left_stick_y);
        float Ch4 = (gamepad1.left_stick_x);


        // note that if y equal -1 then joystick is pushed all of the way forward.
        float rightfront = Ch3 + Ch1 - Ch4;
        float rightback = Ch3 + Ch1 + Ch4;
        float leftfront = Ch3 - Ch1 + Ch4;
        float leftback = Ch3 - Ch1 - Ch4;

// Equation for Drive, Left Stick when pushed forward sends robot forward, Left Stick when sideways controls strafing, Right stick when pushed left controls turning





        // clip the right/left values so that the values never exceed +/- 1
        rightback = Range.clip(rightback, -1, 1);
        leftback = Range.clip(leftback, -1, 1);
        rightfront = Range.clip(rightfront, -1, 1);
        leftfront = Range.clip(leftfront, -1, 1);



        // scale the joystick value to make it easier to control
        // the robot more precisely at slower speeds.
        rightfront = (float)scaleInput(rightfront);
        leftfront =  (float)scaleInput(leftfront);
        rightback = (float)scaleInput(rightback);
        leftback =  (float)scaleInput(leftback);





        // write the values to the motors
        rightdrive.setPower(rightfront);
        leftdrive.setPower(leftfront);
        reallyrightback.setPower(rightback);
        Leftback.setPower(leftback);



        // Sends back values to drive station
    }

    /*
     * Code to run ONCE after the driver hits STOP
     */
    @Override
    public void stop() {
    }
    double scaleInput(double dVal)  {
        double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,
                0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };

        // get the corresponding index for the scaleInput array.
        int index = (int) (dVal * 16.0);

        // index should be positive.
        if (index < 0) {
            index = -index;
        }

        // index cannot exceed size of array minus 1.
        if (index > 16) {
            index = 16;
        }

        // get value from the array.
        double dScale = 0.0;
        if (dVal < 0) {
            dScale = -scaleArray[index];
        } else {
            dScale = scaleArray[index];
        }

        // return scaled value.
        return dScale;
    }

}