/*
Copyright (c) 2016 Robert Atkinson

All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted (subject to the limitations in the disclaimer below) provided that
the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.

Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

Neither the name of Robert Atkinson nor the names of his contributors may be used to
endorse or promote products derived from this software without specific prior
written permission.

NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.LightSensor;
import com.qualcomm.robotcore.hardware.TouchSensor;

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

/**
 * This file illustrates the concept of driving up to a line and then stopping.
 * It uses the common Pushbot hardware class to define the drive on the robot.
 * The code is structured as a LinearOpMode
 * <p>
 * The code shows using two different light sensors:
 * The Primary sensor shown in this code is a legacy NXT Light sensor (called "sensor_light")
 * Alternative "commented out" code uses a MR Optical Distance Sensor (called "sensor_ods")
 * instead of the LEGO sensor.  Chose to use one sensor or the other.
 * <p>
 * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
 * This should be set half way between the light and dark values.
 * These values can be read on the screen once the OpMode has been INIT, but before it is STARTED.
 * Move the senso on asnd off the white line and not the min and max readings.
 * Edit this code to make WHITE_THRESHOLD half way between the min and max.
 * <p>
 * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
 * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
 */

@Autonomous(name = "6417 Auto Gyro Drive", group = "Pushbot")
//@Disabled
public class AutoDriveGyro6417 extends LinearOpMode {

    /* Declare OpMode members. */
    Hardware6417 robot = new Hardware6417();   // Use a Pushbot's hardware
    // could also use HardwarePushbotMatrix class.
    LightSensor frontLight;      // Primary LEGO Light sensor,
    LightSensor backLight;      // Primary LEGO Light sensor,
    ColorSensor colorSensor;    // Hardware Device Object
    TouchSensor touchSensor;
    ModernRoboticsI2cGyro gyro;   // Hardware Device Object
    // OpticalDistanceSensor   frontLight;   // Alternative MR ODS sensor

    boolean redTeam;

    static final double WHITE_THRESHOLD = 0.34;  // spans between 0.1 - 0.5 from dark to light
    static final double BLACK_THRESHOLD = 0.22;
    static final double APPROACH_SPEED = 0.5;
    static final double TURN_SPEED = 0.3;
    static final double DISTANCE = 4.5;

    @Override
    public void runOpMode() throws InterruptedException {

        /* Initialize the drive system variables.
         * The init() method of the hardware class does all the work here
         */
        robot.init(hardwareMap);

        // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
        // robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        // robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        // get a reference to our Light Sensor object.
        frontLight = hardwareMap.lightSensor.get("sensor_light_front");                // Primary LEGO Light Sensor
        backLight = hardwareMap.lightSensor.get("sensor_light_back");                // Primary LEGO Light Sensor
        //  frontLight = hardwareMap.opticalDistanceSensor.get("sensor_ods");  // Alternative MR ODS sensor.

        colorSensor = hardwareMap.colorSensor.get("color sensor"); // get a reference to our ColorSensor object.
        touchSensor = hardwareMap.touchSensor.get("touch sensor");
        gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");// get a reference to a Modern Robotics GyroSensor object.

        // start calibrating the gyro.
        telemetry.addData(">", "Gyro Calibrating. Do Not move!");
        telemetry.update();
        gyro.calibrate();
        gyro.resetZAxisIntegrator();

        // make sure the gyro is calibrated.
        while (!isStopRequested() && gyro.isCalibrating()) {
            sleep(50);
            idle();
        }

        // Set the LED in the beginning
        colorSensor.enableLed(false);

        // turn on LED of light sensor.
        frontLight.enableLed(true);
        backLight.enableLed(true);

        robot.leftHand.setPosition(robot.HAND_MIN);
        robot.rightHand.setPosition(robot.HAND_MAX);

        // bPrevState and bCurrState represent the previous and current state of the button.
        boolean bPrevState = false;
        boolean bCurrState = false;

        // bLedOn represents the state of the LED.
        redTeam = true;

        // Wait for the game to start (driver presses PLAY)
        // Abort this loop is started or stopped.
        while (!(isStarted() || isStopRequested())) {

            // check the status of the x button on either gamepad.
            bCurrState = touchSensor.isPressed();

            // check for button state transitions.
            if ((bCurrState == true) && (bCurrState != bPrevState)) {

                // button is transitioning to a pressed state.
                redTeam = !redTeam;
            }

            // update previous state variable.
            bPrevState = bCurrState;

            // Display the light level while we are waiting to start
            telemetry.addData("Team", redTeam ? "Red" : "Blue");
            telemetry.addData(">", "Gyro Calibrated.");
            telemetry.addData("Heading", gyro.getIntegratedZValue());
            telemetry.addData("Front Light Level", frontLight.getLightDetected());
            telemetry.addData("Status", "Ready to Start");
            telemetry.update();
            idle();
        }

        if (opModeIsActive()) {

            // Launch two balls
            // Move forward a little
            robot.leftMotor.setPower(APPROACH_SPEED);
            robot.rightMotor.setPower(APPROACH_SPEED - .05); //keep straight
            sleep(450);
            robot.leftMotor.setPower(0.0);
            robot.rightMotor.setPower(0.0);
            sleep(200);
            // Launch ball 1
            robot.armMotor.setPower(robot.ARM_UP_POWER);
            sleep(1800);
            robot.armMotor.setPower(0.0);

            sleep(300);
            // Launch ball 2
            robot.legMotor.setPower(robot.LEG_UP_POWER); // Serve ball 2
            sleep(1800); // Let ball come to a rest
            robot.legMotor.setPower(0.0);
            sleep(500);
            robot.armMotor.setPower(robot.ARM_UP_POWER);
            sleep(800);
            robot.armMotor.setPower(0.0);

            // Rotate towards white line

            if (redTeam) {
                rotate(35);
            } else {
                rotate(-35);
            }

            //sleep(5000);

            // Start the robot moving forward, and then begin looking for a white line.
            robot.leftMotor.setPower(APPROACH_SPEED);
            robot.rightMotor.setPower(APPROACH_SPEED);
            sleep(1000);

            // run until the white line is seen OR the driver presses STOP;
            while (opModeIsActive() && (frontLight.getLightDetected() < WHITE_THRESHOLD)) {

                // Display the light level while we are looking for the line
                telemetry.addData("Light Level", frontLight.getLightDetected());
                telemetry.update();
                idle();

            }

            sleep(100);
            //back up a little so we can hit the center of first beacon
            robot.rightMotor.setPower(-APPROACH_SPEED);
            robot.leftMotor.setPower(-APPROACH_SPEED);
            sleep(620);

            //rotate towards beacon
            if (redTeam) {
                rotate(86);
            } else {
                rotate(-86);
            }

            /*if (redTeam)
                robot.rightMotor.setPower(APPROACH_SPEED);
            else
                robot.leftMotor.setPower(APPROACH_SPEED);

            // Stop all motors
            //robot.leftMotor.setPower(0);
            //robot.rightMotor.setPower(0);

            // run until the white line is seen OR the driver presses STOP;
            while (opModeIsActive() && (backLight.getLightDetected() < WHITE_THRESHOLD)) {

                // Display the light level while we are looking for the line
                telemetry.addData("Light Level", backLight.getLightDetected());
                telemetry.update();
                idle();

            }

            try {
                Thread.sleep(300);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }


            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);

            try {
                Thread.sleep(500);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            */

            //approach beacon
            approachBeacon();

            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);
            sleep(400);

            pressBeacon();

            //backup from beacon in preparation of turn
            robot.rightMotor.setPower(-APPROACH_SPEED);
            robot.leftMotor.setPower(-APPROACH_SPEED);
            sleep(500);

            //rotate towards second beacon (values offset to account for variance)
            if (redTeam) {
                rotate(3);
            } else {
                rotate(-3);
            }
           // rotate(0);

            // Start the robot moving forward, and then begin looking for second white line.
            robot.leftMotor.setPower(APPROACH_SPEED);
            robot.rightMotor.setPower(APPROACH_SPEED);
            sleep(2000);

            // run until the second white line is seen OR the driver presses STOP;
            while (opModeIsActive() && (frontLight.getLightDetected() < WHITE_THRESHOLD)) {

                // Display the light level while we are looking for the line
                telemetry.addData("Light Level", frontLight.getLightDetected());
                telemetry.update();
                idle();

            }

            sleep(100);
            //back up a little so we can hit the center of second beacon
            robot.rightMotor.setPower(-APPROACH_SPEED);
            robot.leftMotor.setPower(-APPROACH_SPEED);
            sleep(250);

            //rotate towards beacon
            if (redTeam) {
                rotate(86);
            } else {
                rotate(-86);
            }

            //approach second beacon
            approachBeacon();

            robot.leftMotor.setPower(0);
            robot.rightMotor.setPower(0);
            sleep(400);

            pressBeacon();
            sleep(500); //retract final

        }

        telemetry.addData("Status", "Done");
        telemetry.update();

    }

    public void approachBeacon(){

        while (opModeIsActive() && (robot.rangeSensor.getDistance(DistanceUnit.CM) > DISTANCE)) {

            double fLight = frontLight.getLightDetected();
            double bLight = backLight.getLightDetected();
            double left = APPROACH_SPEED;
            double right = APPROACH_SPEED;

            robot.rightMotor.setPower(left); //left
            robot.leftMotor.setPower(right);

            // Display the light level while we are looking for the line
            //telemetry.addData("Front Light Level",  fLight);
            //telemetry.addData("Back Light Level",  bLight);
            telemetry.addData("cm", "%.2f cm", robot.rangeSensor.getDistance(DistanceUnit.CM));
            telemetry.update();

        }

    }

    public void pressBeacon() {

        telemetry.addData("Red  ", robot.colorSensor.red());
        telemetry.addData("Blue ", robot.colorSensor.blue());
        telemetry.update();

        int stillBlue = robot.colorSensor.blue() - robot.colorSensor.red(); //stillblue if +

        if (redTeam == (stillBlue > 0)) {

            robot.rightHand.setPosition(robot.HAND_MIN);

        } else if (stillBlue != 0) {

            robot.leftHand.setPosition(robot.HAND_MAX);

        }

        //if color was found
        if(stillBlue != 0)
            sleep(1000);

        robot.leftHand.setPosition(robot.HAND_MIN);
        robot.rightHand.setPosition(robot.HAND_MAX);

    }

    public double trim(double value, double min, double max) {
        return Math.min(Math.max(value, min), max);
    }

    public void rotate(int target) {

        int angle = gyro.getIntegratedZValue(); //use double for floating point precision
        telemetry.addData("Heading", angle);
        telemetry.addData("Target", target);
        telemetry.update();

        double totalDelta = Math.abs((target - angle) / 2.0); //full turn speed until halfway there

        //positive is counter-clockwise (left)
        //error tolerance of 2.5 degrees
        while (opModeIsActive() && Math.abs(angle - target) > 2.5) {

            double currentDelta = Math.abs(target - angle);
            double turnRate = trim(currentDelta / totalDelta, .15, TURN_SPEED); //trim speed within acceptable range

            if (angle < target) {

                //left turn
                robot.leftMotor.setPower(-turnRate);
                robot.rightMotor.setPower(turnRate);

            } else {

                //right turn
                robot.leftMotor.setPower(turnRate);
                robot.rightMotor.setPower(-turnRate);

            }

            angle = gyro.getIntegratedZValue();
            telemetry.addData("Target", target);
            telemetry.addData("Heading", angle);
            telemetry.update();

        }

        /* if(angle < target){

        } else {

            while (angle > target) {

                int currentDelta = target - angle;
                double turnRate = trim(currentDelta / totalDelta, .15, 1) * TURN_SPEED;

                robot.leftMotor.setPower(turnRate);
                robot.rightMotor.setPower(-turnRate);

                angle = gyro.getIntegratedZValue();
                telemetry.addData("Target", target);
                telemetry.addData("Heading", angle);
                telemetry.update();

            }

        }*/

        robot.leftMotor.setPower(0);
        robot.rightMotor.setPower(0);
        sleep(300);

    }

}