/* Copyright (c) 2015 Qualcomm Technologies Inc

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 Qualcomm Technologies Inc nor the names of its 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 FITNESS FOR 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 android.app.Activity;
import android.graphics.Color;
import android.view.View;

import com.qualcomm.ftcrobotcontroller.R;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cColorSensor;
import com.qualcomm.robotcore.util.ElapsedTime;

/*
 *
 * This is an example LinearOpMode that tests the update speed of the gyro sensor
 * when a color sensor is enabled for callbacks and disabled.
 *
 * The op mode assumes that the color sensor
 * is configured with a name of "color" and the gyro with a name of "gyro".
 *
 * You can use the X button on gamepad1 to toggle the color's sensor's LED state.
 * Turning the LED off disables the color sensor's callback.
 *
 */
@TeleOp(name = "Gyro Heading with Color Sensor", group = "Sensor")
public class Team5873_SensorMRColorEnableDisable extends LinearOpMode {


  public void runOpMode() {

    ElapsedTime timer = new ElapsedTime();
    timer.reset();

    // The time since the color or gyro sensor was last updated
    double dPrevColorMillis = 0;
    double dCurrColorMillis = 0;
    double dPrevGyroMillis = 0;
    double dCurrGyroMillis = 0;

    // The previous and current values of the color sensor (red only) and gyro heading
    double dPrevRed = 0.0;
    double dCurrRed = 0.0;
    int iPrevAngleZ = 0;
    int iCurrAngleZ = 0;

    // The previous and current state of gamepad1's X button.
    boolean bPrevState = false;
    boolean bCurrState = false;

    // The state of the LED on the color sensor.
    boolean bLedOn = true;

    // get a reference to our ColorSensor object.
    ModernRoboticsI2cColorSensor colorSensor = (ModernRoboticsI2cColorSensor)this.hardwareMap.get("color");
    colorSensor.resetDeviceConfigurationForOpMode();

    // Set the LED to ON / true in the beginning
    colorSensor.enableLed(bLedOn);

    ModernRoboticsI2cGyro gyro;   // Hardware Device Object

    // get a reference to a Modern Robotics GyroSensor object.
    gyro = (ModernRoboticsI2cGyro)hardwareMap.gyroSensor.get("gyro");

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

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

    telemetry.addData(">", "Gyro Calibrated.  Press Start.");
    telemetry.update();

    waitForStart();

    // while the op mode is active, loop and read the RGB data.
    // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
    while (opModeIsActive()) {

      bCurrState = gamepad1.x;
      dCurrColorMillis = timer.milliseconds();
      dCurrGyroMillis = dCurrColorMillis;
      dCurrRed = colorSensor.red() * 1.0;
      iCurrAngleZ  = gyro.getIntegratedZValue();

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

        // button is transitioning its state. So toggle the bLedOn
        bLedOn = !bLedOn;
        // Enable or disable the color sensor and LED at the same time
        if (bLedOn) {
          colorSensor.getI2cController().registerForI2cPortReadyCallback(colorSensor, colorSensor.getPort());
          colorSensor.enableLed(bLedOn);
        }
        else {
          colorSensor.enableLed(bLedOn);
          colorSensor.getI2cController().deregisterForPortReadyCallback(colorSensor.getPort());
        }
      }

      // update previous state variable.
      bPrevState = bCurrState;

      // send the info back to driver station using telemetry function.
      telemetry.addData("LED/Color Callback", bLedOn ? "On" : "Off");
      telemetry.addData("Clear", colorSensor.alpha());
      telemetry.addData("R/G/B", (Double.toString(dCurrRed) + "/" + Double.toString(colorSensor.green()) + "/" + Double.toString(colorSensor.blue())));

      // If the red value changes, show the milliseconds since last update
      // Write a line of telemetry in any case so the values don't jump around the screen
      if (dPrevRed == dCurrRed) {
        telemetry.addData("No Color Update", 0);
      }
      else {
        telemetry.addData("Color Update Time", dCurrColorMillis - dPrevColorMillis);
        dPrevColorMillis = dCurrColorMillis;
        dPrevRed = dCurrRed;
      }

      // If the heading changes, show the milliseconds since last update
      telemetry.addData("1", "Int. Ang. %03d", iCurrAngleZ);
      if (iPrevAngleZ == iCurrAngleZ) {
        telemetry.addData("No Heading Update", 0);
      }
      else {
        telemetry.addData("Heading Update Time", dCurrGyroMillis - dPrevGyroMillis);
        dPrevGyroMillis = dCurrGyroMillis;
        iPrevAngleZ = iCurrAngleZ;
      }

      telemetry.update();
    }
  }
}