package org.ftc8702.opmodes.production;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.Range;

import org.ftc8702.components.motors.MecanumWheelDriveTrain;
import org.ftc8702.configurations.production.ProdMecanumRobotConfiguration;

import ftcbootstrap.ActiveOpMode;

@TeleOp(name = "SkystoneTeleOp", group = "production")
public class SkystoneTeleOp extends ActiveOpMode {

    private ProdMecanumRobotConfiguration driveTrainConfig;
    private MecanumWheelDriveTrain driveTrain;

    /**
     * Implement this method to define the code to run when the Init button is pressed on the Driver station.
     */
    @Override
    protected void onInit() {

        driveTrainConfig = ProdMecanumRobotConfiguration.newConfig(hardwareMap, getTelemetryUtil());

        //Note The Telemetry Utility is designed to let you organize all telemetry data before sending it to
        //the Driver station via the sendTelemetry command
        getTelemetryUtil().addData("Init", getClass().getSimpleName() + " initialized.");

    }

    @Override
    protected void onStart() throws InterruptedException {
        super.onStart();
        driveTrain = new MecanumWheelDriveTrain(driveTrainConfig.motorFL, driveTrainConfig.motorFR, driveTrainConfig.motorBL, driveTrainConfig.motorBR);
    }

    /**
     * Implement this method to define the code to run when the Start button is pressed on the Driver station.
     * This method will be called on each hardware cycle just as the loop() method is called for event based Opmodes
     *
     * @throws
     */
    @Override
    protected void activeLoop() throws InterruptedException {
        if (gamepad1.right_bumper)
        {
            driveTrain.rotateRight(1);
        }
        else if (gamepad1.left_bumper)
        {
            driveTrain.rotateLeft(1);
        }
        else if (gamepad1.left_stick_y != 0)
        {
            float scaledPower = scaleMotorPower(gamepad1.left_stick_y);
            driveTrain.goForward(scaledPower);
        }
        else if (gamepad1.left_stick_x != 0)
        {
            float scaledPower = scaleMotorPower(gamepad1.left_stick_x);
            driveTrain.strafeRight(scaledPower);
        }
        else
        {
            driveTrain.stop();
        }

        getTelemetryUtil().addData("Left Joystick Y-Power: ", gamepad1.left_stick_y);
        getTelemetryUtil().sendTelemetry();
    }

    /**
     * Taken from FTC SDK PushBot example
     * The DC motors are scaled to make it easier to control them at slower speeds
     * Obtain the current values of the joystick controllers.
     * Note that x and y equal -1 when the joystick is pushed all of the way
     * forward (i.e. away from the human holder's body).
     * The clip method guarantees the value never exceeds the range +-1.
     */
    private float scaleMotorPower(float p_power) {

        // Assume no scaling.
        float l_scale = 0.0f;

        // Ensure the values are legal.
        float l_power = Range.clip(p_power, -1, 1);

        float[] l_array =
                {0.00f, 0.05f, 0.09f, 0.10f, 0.12f
                        , 0.15f, 0.18f, 0.24f, 0.30f, 0.36f
                        , 0.43f, 0.50f, 0.60f, 0.72f, 0.85f
                        , 1.00f, 1.00f
                };

        int l_index = (int) (l_power * 16.0);
        if (l_index < 0) {
            l_index = -l_index;
        } else if (l_index > 16) {
            l_index = 16;
        }

        if (l_power < 0) {
            l_scale = -l_array[l_index];
        } else {
            l_scale = l_array[l_index];
        }

        return l_scale;

    }
}

@@ -1696,32 +1696,131 @@
 mper)%0A        %7B%0A
+            float power = 1;%0A            getTelemetryUtil().addData(%22Right bumper power:%22, power);%0A
             driv
@@ -1838,17 +1838,21 @@
 teRight(
-1
+power
 );%0A     
@@ -1897,32 +1897,130 @@
 mper)%0A        %7B%0A
+            float power = 1;%0A            getTelemetryUtil().addData(%22Left bumper power:%22, power);%0A
             driv
@@ -2037,17 +2037,21 @@
 ateLeft(
-1
+power
 );%0A     
@@ -2174,32 +2174,180 @@
 .left_stick_y);%0A
+            getTelemetryUtil().addData(%22Left Joystick Y: %22,%0A                    %22value=%22 + gamepad1.left_stick_y + %22, scaledPower=%22 + scaledPower);%0A
             driv
@@ -2514,16 +2514,164 @@
 ick_x);%0A
+            getTelemetryUtil().addData(%22Left Joystick X: %22,%0A                    %22value=%22 + gamepad1.left_stick_x + %22, scaledPower=%22 + scaledPower);%0A
         
@@ -2790,94 +2790,8 @@
  %7D%0A%0A
-        getTelemetryUtil().addData(%22Left Joystick Y-Power: %22, gamepad1.left_stick_y);%0A
     

add telemetry message to teleop