package com.qualcomm.ftcrobotcontroller.opmodes.customCode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;

public class Autonomous extends ETBaseOpMode {
  private DcMotor right;
  private DcMotor left;

  final static int ENCODER_CPR = 1440;    //encoder counts per revolution
  final static double GEAR_RATIO = 1;     //gear ratio
  final static double WHEEL_DIAMETER = 2.625;     //diameter of wheel
  final static double CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER;
  //int distance = 5;
  final static int ERROR_THRESHOLD = 10;

  private static boolean isTargetSet = false;
  private static  double counts = 0;

  private static int stage = 20;

  @Override
  public void etSetup() throws InterruptedException {
    right = hardwareMap.dcMotor.get("Right");
    left = hardwareMap.dcMotor.get("Left");
    right.setDirection(DcMotor.Direction.REVERSE);

  }


  @Override
  public void etLoop() throws InterruptedException {
    /*
    telemetry.addData("Left Position", left.getCurrentPosition());
    telemetry.addData("Right Position", right.getCurrentPosition());
    telemetry.addData("Right Motor Power", Double.toString(right.getPower()));
    telemetry.addData("Left Motor Power", Double.toString(left.getPower()));
*/

    telemetry.addData("stage", stage);

    switch (stage) {
      case 20 :
      {
        counts = getCountsForDistance(20);
        telemetry.addData("stage 20", counts);

        setTarget();
        if(hasArrived(counts))
          stage = 10;
        break;
      }
      case 10: {
        counts = getCountsForDistance(10);

        setTarget();
        if(hasArrived(counts))
          stage = 100;
        break;

      }

      case 100: {
        stopRobot();
        break;
      }
    }
  }

  private boolean hasArrived(double cts) {
    //telemetry.addData("cts", cts);
    telemetry.addData("Left Position", left.getCurrentPosition());
    telemetry.addData("Right Position", right.getCurrentPosition());
    telemetry.addData("cts", cts);

    int rightErrorMargin = Math.abs(Math.abs(right.getCurrentPosition()) - (int) cts);
    int leftErrorMargin = Math.abs(Math.abs(right.getCurrentPosition()) - (int) cts);
    if (leftErrorMargin < ERROR_THRESHOLD && rightErrorMargin < ERROR_THRESHOLD) {
      isTargetSet = false;
      telemetry.addData("hasArrived", cts);
      return true;
    }
    return false;

  }

  private void stopRobot() throws InterruptedException {
    right.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    left.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    waitOneFullHardwareCycle();
    etBreakLoop();
  }

  private double getCountsForDistance(int distance) {
    double rotations = distance / CIRCUMFERENCE;
    return ENCODER_CPR * rotations * GEAR_RATIO;
  }

  private synchronized void  setTarget() throws InterruptedException {
    if(isTargetSet)
      return;

    //telemetry.addData("setTarget ", distance);

    //double cts = getCountsForDistance(distance);
    right.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    left.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    waitOneFullHardwareCycle();
    telemetry.addData("Reset Encoders", "Done");

    right.setTargetPosition((int) counts);
    left.setTargetPosition((int) counts);
    right.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    left.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    right.setPower(0.25);
    left.setPower(0.25);
    isTargetSet = true;
    //return cts;
  }

}

@@ -644,16 +644,186 @@
 LD = 10;
+%0A  final static int STAGE_MOVE_20 = 20;%0A  final static int STAGE_MOVE_10 = 10;%0A  final static int STAGE_TURN_LEFT = 90; // case 90 :)%0A  final static int STAGE_STOP = 100;
 %0A%0A  priv
@@ -879,17 +879,16 @@
  static 
- 
 double c
@@ -925,21 +925,16 @@
 nt stage
- = 20
 ;%0A%0A  @Ov
@@ -960,13 +960,12 @@
 d et
-Setup
+Init
 () t
@@ -1133,16 +1133,114 @@
 EVERSE);
+%0A  %7D%0A%0A  @Override%0A  public void etSetup() throws InterruptedException %7B%0A    stage = STAGE_MOVE_20;
 %0A%0A  %7D%0A%0A%0A
@@ -1679,16 +1679,27 @@
    case 
+STAGE_MOVE_
 20 :%0A   
@@ -1864,16 +1864,27 @@
 stage = 
+STAGE_MOVE_
 10;%0A    
@@ -2033,19 +2033,26 @@
 stage = 
-100
+STAGE_STOP
 ;%0A      
@@ -2081,19 +2081,26 @@
    case 
-100
+STAGE_STOP
 : %7B%0A    

Update autonomous to fix stage 10 bug, new stage names