package org.ligerbots.steamworks.commands;

import org.ligerbots.steamworks.Robot;
import org.ligerbots.steamworks.RobotMap;
import org.ligerbots.steamworks.subsystems.DriveTrain;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * This command turns the robot by a certain number of degrees. Clockwise is positive (NavX
 * convention)
 */
public class TurnCommand extends AccessibleCommand {
  private static final Logger logger = LoggerFactory.getLogger(TurnCommand.class);

  double offsetDegrees;
  double maxTime; // seconds
  long startTime;
  double startingRotation;
  double targetRotation;
  boolean isClockwise;
  double error2;
  double error1;

  // did we end up where we want or was it aborted?
  boolean succeeded;
  boolean ended;
  
  boolean isHighGear;
  
  double autoTurnRampZone;
  double autoTurnMaxSpeed;
  double autoTurnMinSpeed;

  /**
   * Create a new TurnCommand.
   * 
   * @param offsetDegrees The number of degrees to turn by. Clockwise is positive
   */
  public TurnCommand(double offsetDegrees) {
    super("TurnCommand_" + offsetDegrees);
    requires(Robot.driveTrain);
    this.offsetDegrees = offsetDegrees;
    maxTime = 2.5 * (offsetDegrees / 180);
    // give at least 5 seconds for turning
    if (maxTime < 5) {
      maxTime = 5;
    }
    isClockwise = offsetDegrees > 0 && offsetDegrees <= 180;
  }

  // Called just before this Command runs the first time
  protected void initialize() {
    startTime = System.nanoTime();
    startingRotation = Robot.driveTrain.getYaw();
    targetRotation = DriveTrain.fixDegrees(startingRotation + offsetDegrees);
    logger.debug(String.format("Start %f, target %f", startingRotation, targetRotation));
    succeeded = false;
    ended = false;
    Robot.driveTrain.shift(DriveTrain.ShiftType.DOWN);
    isHighGear = false;
    
    if (isHighGear) {
      autoTurnRampZone = RobotMap.AUTO_TURN_RAMP_ZONE_HIGH;
      autoTurnMaxSpeed = RobotMap.AUTO_TURN_MAX_SPEED_HIGH;
      autoTurnMinSpeed = RobotMap.AUTO_TURN_MIN_SPEED_HIGH;
    } else {
      autoTurnRampZone = RobotMap.AUTO_TURN_RAMP_ZONE_LOW;
      autoTurnMaxSpeed = RobotMap.AUTO_TURN_MAX_SPEED_LOW;
      autoTurnMinSpeed = RobotMap.AUTO_TURN_MIN_SPEED_LOW;
    }
  }
  
  // Called repeatedly when this Command is scheduled to run
  protected void execute() {
    // We could be turning clockwise or counterclockwise, so an "error" of 350deg for example, is
    // actually an error of 10deg
    error1 = Math.abs(targetRotation - Robot.driveTrain.getYaw());
    error2 = Math.abs(360 - error1);
    double actualError = Math.min(error1, error2);
    if (actualError <= autoTurnRampZone) {
      double driveSpeed = autoTurnMaxSpeed * actualError / autoTurnRampZone;
      if (driveSpeed < autoTurnMinSpeed) {
        driveSpeed = autoTurnMinSpeed;
      }
      Robot.driveTrain.rawThrottleTurnDrive(0, isClockwise ? -driveSpeed : driveSpeed);
    } else {
      Robot.driveTrain.rawThrottleTurnDrive(0,
          isClockwise ? -autoTurnMaxSpeed : autoTurnMaxSpeed);
    }
  }

  protected boolean isFinished() {
    boolean outOfTime = System.nanoTime() - startTime > (maxTime * RobotMap.NANOS_PER_SECOND);
    logger.debug(
        String.format("Error %f %f, absolute yaw %f", error1, error2, Robot.driveTrain.getYaw()));
    boolean onTarget = error1 < RobotMap.AUTO_TURN_ACCEPTABLE_ERROR
        || error2 < RobotMap.AUTO_TURN_ACCEPTABLE_ERROR;
    if (onTarget) {
      succeeded = true;
    }
    ended = outOfTime || onTarget || Robot.operatorInterface.isCancelled();
    return ended;
  }

  protected void end() {
    logger.info("Finish");
    Robot.driveTrain.rawThrottleTurnDrive(0, 0);
    ended = true;
  }

  protected void interrupted() {
    logger.warn("Interrupted");
    Robot.driveTrain.rawThrottleTurnDrive(0, 0);
    ended = true;
  }
  
  protected boolean isFailedToComplete() {
    return ended && !succeeded;
  }
}

@@ -1115,16 +1115,154 @@
 Train);%0A
+    offsetDegrees = DriveTrain.fixDegrees(offsetDegrees);%0A    if (offsetDegrees %3E 180) %7B%0A      offsetDegrees = offsetDegrees - 360;%0A    %7D%0A
     this
@@ -1313,16 +1313,25 @@
 = 2.5 * 
+(Math.abs
 (offsetD
@@ -1336,16 +1336,17 @@
 tDegrees
+)
  / 180);
@@ -1476,32 +1476,8 @@
  %3E 0
- && offsetDegrees %3C= 180
 ;%0A  
@@ -1626,16 +1626,38 @@
 ation = 
+DriveTrain.fixDegrees(
 Robot.dr
@@ -1673,16 +1673,17 @@
 getYaw()
+)
 ;%0A    ta
@@ -1840,24 +1840,29 @@
 Rotation));%0A
+    %0A
     succeede
@@ -2466,24 +2466,316 @@
 execute() %7B%0A
+    double localTargetRotation = targetRotation;%0A    double currentRotation = DriveTrain.fixDegrees(Robot.driveTrain.getYaw());%0A    %0A    if (localTargetRotation %3C currentRotation) %7B%0A      localTargetRotation += 360;%0A    %7D%0A%0A    isClockwise = localTargetRotation - currentRotation %3C= 180;%0A    %0A
     // We co
@@ -2937,33 +2937,23 @@
 n - 
-Robot.driveTrain.getYaw()
+currentRotation
 );%0A 

Make turncommand not wildly rotate