package org.strykeforce.thirdcoast.robot;

import com.ctre.CANTalon;
import com.electronwill.nightconfig.core.file.FileConfig;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.strykeforce.thirdcoast.swerve.SwerveDrive;
import org.strykeforce.thirdcoast.talon.StatusFrameRate;
import org.strykeforce.thirdcoast.telemetry.TelemetryService;

/**
 * Third Coast swerve drive demo robot.
 */

public class Robot extends IterativeRobot {

  final static Logger logger = LoggerFactory.getLogger(Robot.class);
  private final static String CONFIG = "/home/lvuser/thirdcoast.toml";
  private final static String DEFAULT_CONFIG = "/org/strykeforce/thirdcoast/defaults.toml";

  private TelemetryService telemetryService;
  private SwerveDrive swerve;
  private Controls controls;
  private final Trigger gyroResetButton = new Trigger() {
    @Override
    public boolean get() {
      return controls.getResetButton();
    }

    @Override
    public String toString() {
      return "gyro reset button";
    }
  };
  private final Trigger alignWheelsButton = new Trigger() {
    @Override
    public boolean get() {
      return controls.getGamepadBackButton() && controls.getGamepadStartButton();
    }

    @Override
    public String toString() {
      return "wheel alignment button combination";
    }
  };

  @Override
  public void robotInit() {
    logger.info("Robot is initializing");
    try {
      RobotComponent component;
      try (FileConfig toml = FileConfig.builder(CONFIG).defaultResource(DEFAULT_CONFIG).build()) {
        logger.info("loading robot configuration from {}", CONFIG);
        toml.load();
        component = DaggerRobotComponent.builder().toml(toml.unmodifiable()).build();
      }
      controls = component.controls();
      swerve = component.swerveDrive();
      telemetryService = component.telemetryService();
//      telemetryService.register(new CANTalon(6));
      swerve.registerWith(telemetryService);
//      StatusFrameRate rates = StatusFrameRate.builder().general(5).feedback(5).build();
//      telemetryService.configureStatusFrameRates(6, rates);
      telemetryService.start();
      swerve.zeroAzimuthEncoders();
    } catch (Throwable t) {
      logger.error("Error initializing robot", t);
    }
  }

  @Override
  public void teleopInit() {
    logger.info("Robot is enabled in tele-op");
    swerve.stop();
  }

  @Override
  public void teleopPeriodic() {
    if (gyroResetButton.hasActivated()) {
      String msg = "Resetting gyro yaw zero";
      logger.warn(msg);
      DriverStation.reportWarning(msg, false);
      swerve.getGyro().zeroYaw();
    }
    double forward = applyDeadband(controls.getForward());
    double strafe = applyDeadband(controls.getStrafe());
    double azimuth = applyDeadband(controls.getAzimuth());

    swerve.drive(forward, strafe, azimuth);
  }

  @Override
  public void disabledInit() {
    logger.info("Robot is disabled");
  }

  @Override
  public void disabledPeriodic() {
    if (alignWheelsButton.hasActivated()) {
      swerve.saveAzimuthPositions();
      swerve.zeroAzimuthEncoders();
      String msg = "drive wheels were re-aligned";
      logger.info(msg);
      DriverStation.reportWarning(msg, false);
    }
  }

  private double applyDeadband(double input) {
    if (Math.abs(input) < 0.05) {
      return 0;
    }
    return input;
  }

}

@@ -40,34 +40,8 @@
 t;%0A%0A
-import com.ctre.CANTalon;%0A
 impo
@@ -94,16 +94,16 @@
 Config;%0A
+
 import e
@@ -298,65 +298,8 @@
 ve;%0A
-import org.strykeforce.thirdcoast.talon.StatusFrameRate;%0A
 impo
@@ -1412,354 +1412,33 @@
     
-try %7B%0A      RobotComponent component;%0A      try (FileConfig toml = FileConfig.builder(CONFIG).defaultResource(DEFAULT_CONFIG).build()) %7B%0A        logger.info(%22loading robot configuration from %7B%7D%22, CONFIG);%0A        toml.load();%0A        component = DaggerRobotComponent.builder().toml(toml.unmodifiable()).build();%0A      %7D%0A      controls = component
+controls = getComponent()
 .con
@@ -1442,26 +1442,24 @@
 controls();%0A
-  
     swerve =
@@ -1459,25 +1459,28 @@
 werve = 
-c
+getC
 omponent
 .swerveD
@@ -1471,16 +1471,18 @@
 omponent
+()
 .swerveD
@@ -1489,26 +1489,24 @@
 rive();%0A    
- 
- 
 telemetrySer
@@ -1516,17 +1516,20 @@
 e = 
-c
+getC
 omponent
 .tel
@@ -1524,16 +1524,18 @@
 omponent
+()
 .telemet
@@ -1548,65 +1548,11 @@
 ice(
-);%0A//      telemetryService.register(new CANTalon(6)
 );%0A
-  
     
@@ -1594,162 +1594,8 @@
 e);%0A
-//      StatusFrameRate rates = StatusFrameRate.builder().general(5).feedback(5).build();%0A//      telemetryService.configureStatusFrameRates(6, rates);%0A  
     
@@ -1616,26 +1616,24 @@
 ce.start();%0A
-  
     swerve.z
@@ -1658,93 +1658,8 @@
 ();%0A
-    %7D catch (Throwable t) %7B%0A      logger.error(%22Error initializing robot%22, t);%0A    %7D%0A
   %7D%0A
@@ -2723,24 +2723,24 @@
 rn 0;%0A    %7D%0A
-
     return i
@@ -2750,10 +2750,381 @@
 t;%0A  %7D%0A%0A
+  private RobotComponent getComponent() %7B%0A    RobotComponent component;%0A    try (FileConfig toml = FileConfig.builder(CONFIG).defaultResource(DEFAULT_CONFIG).build()) %7B%0A      logger.info(%22loading robot configuration from %7B%7D%22, CONFIG);%0A      toml.load();%0A      component = DaggerRobotComponent.builder().toml(toml.unmodifiable()).build();%0A    %7D%0A    return component;%0A  %7D%0A%0A
 %7D%0A

Extract component code to method