package ChassisSolution;

//import MotorControlSolution.*;
import MotorControlSolution.*;

import javax.swing.*;
import java.awt.*;
import java.awt.event.*;

/**
 * <p>Entry point for chassis lab.</p>
 **/
public class Chassis {

  /**
   * <p>The robot.</p>
   **/
  public static OdometryRobot robot = new OdometryRobot();

  /**
   * <p>Entry point for the chassis lab.</p>
   *
   * @param args command line arguments
   **/
  public static void main(String [] args) {

    //////////////////// create velocity controller //////////////////////    

    RobotVelocityController robotVelocityController =
      new RobotVelocityControllerBalanced();

    robot.setRobotVelocityController(robotVelocityController);


    //////////////////// create position controller /////////////////////    

    RobotPositionController robotPositionController =
      new RobotPositionController(robot);

    robot.setRobotPositionController(robotPositionController);


    //////////////////// config controllers /////////////////////////////    

    //if your velocity and/or position controllers need to be configured
    //(i.e. gains set, etc), then do it here

    //this block to configures *our* solution to lab 2 (yours may or
    //may not be configured the same way)
    final double VELOCITY_BALANCE_GAIN = 1.0; 
    final double VELOCITY_WHEEL_GAIN = 6.0;
    robotVelocityController.setGain(VELOCITY_BALANCE_GAIN);
    robotVelocityController.getWheelVelocityController(RobotBase.LEFT). 
      setGain(VELOCITY_WHEEL_GAIN);
    robotVelocityController.getWheelVelocityController(RobotBase.RIGHT).
      setGain(VELOCITY_WHEEL_GAIN);

    final double POSITION_GAIN = 6.0; // (Solution)
    robotPositionController.setGain(POSITION_GAIN); // (Solution)


    //////////////////// display estop button //////////////////////////    

    EstopButton estop = new EstopButton(Thread.currentThread());


    //////////////////// command motion ////////////////////////////////    

    // Begin Student Code
    if(args.length != 1){ // (Solution)
    	System.out.println("Please give a single flag, -t, -r, -b, or -s."); // (Solution)
	robotPositionController.translate(0.2, 1.0); // (Solution)
	robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
	robotPositionController.translate(0.2, 1.0); // (Solution)
	robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
	robotPositionController.translate(0.2, 1.0); // (Solution)
	robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
	robotPositionController.translate(0.2, 1.0); // (Solution)
	robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    } else{ // (Solution)
    	System.out.println("argument: "+args[0]); // (Solution)
    	if(args[0].equals("-t")){ // (Solution)
    		robotPositionController.translate(0.2, 0.5); // (Solution)
    		robotPositionController.translate(0.2, -0.5); // (Solution)
    		robotPositionController.translate(0.4, 0.5); // (Solution)
    		robotPositionController.translate(0.4, -0.5); // (Solution)
    	} else if(args[0].equals("-r")) { // (Solution)
    		robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    		robotPositionController.rotate(0.2, -Math.PI/2.0); // (Solution)
    		robotPositionController.rotate(0.4, Math.PI/2.0); // (Solution)
    		robotPositionController.rotate(0.4, -Math.PI/2.0); // (Solution)
    	} else if(args[0].equals("-b")){ // (Solution)
    		robotPositionController.translate(0.2, 1.0); // (Solution)
    		robotPositionController.rotate(0.2, Math.PI); // (Solution)
    		robotPositionController.translate(0.2, 1.0); // (Solution)
    		robotPositionController.rotate(0.2, -Math.PI); // (Solution)
    	} else if(args[0].equals("-s")) { // (Solution)
    		for (int i = 0; i < 1; i++) { // (Solution)
    			robotPositionController.translate(0.2, 1.0); // (Solution)
    			robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    			robotPositionController.translate(0.2, 1.0); // (Solution)
    			robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    			robotPositionController.translate(0.2, 1.0); // (Solution)
    			robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    			robotPositionController.translate(0.2, 1.0); // (Solution)
    			robotPositionController.rotate(0.2, Math.PI/2.0); // (Solution)
    		} // (Solution)
    	} else { // (Solution)
    		System.out.println("Argument not recognized. Write -t to translate forward and back, -r to rotate in place, -b to both translate and rotate, and -s to drive in a square"); // (Solution)
    	} // (Solution)
    } // (Solution)
    // End Student Code

    //////////////////// shutdown //////////////////////////////////////    

    //robot should already be stopped, but just in case
    robot.estop();
    System.exit(0);
  }

  /**
   * <p>The estop button.</p>
   **/
  protected static class EstopButton extends JDialog {
    
    EstopButton(final Thread mainThread) {

      JButton eb = new JButton("ESTOP");
      eb.setBackground(Color.RED);
      eb.setPreferredSize(new Dimension(200, 200));
      eb.addActionListener(new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            mainThread.interrupt();
            robot.estop();
            System.exit(-1);
          }
        });

      setContentPane(eb);
      setTitle("Emergency Stop");
      setDefaultCloseOperation(JDialog.DO_NOTHING_ON_CLOSE);
      pack();
      setVisible(true);
    }
  }
}