/*
 * Copyright (c) 2017 Titan Robotics Club (http://www.titanrobotics.com)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

package team492;

import trclib.TrcEvent;
import trclib.TrcRobot;
import trclib.TrcStateMachine;
import trclib.TrcTimer;

class CmdVisionGearDeploy implements TrcRobot.RobotCommand
{
    private enum State
    {
        DO_DELAY,
        ALIGN_WITH_TARGET,
        DEPLOY_GEAR,
        BACKUP,
        DONE
    }   //enum State

    private static final String moduleName = "CmdPidDrive";

    private Robot robot;
    private double delay;
    private double extendTime;
    private double xDistance;
    private double yDistance;
    private double heading;
    private TrcEvent event;
    private TrcTimer timer;
    private TrcStateMachine<State> sm;

    CmdVisionGearDeploy(Robot robot, double delay, double extendTime, double xDistance, double yDistance, double heading)
    {
        this.robot = robot;
        this.delay = delay;
        this.extendTime = extendTime;
        this.xDistance = xDistance;
        this.yDistance = yDistance;
        this.heading = heading;
        event = new TrcEvent(moduleName);
        timer = new TrcTimer(moduleName);
        sm = new TrcStateMachine<>(moduleName);
        sm.start(State.DO_DELAY);
    }   //CmdPidDrive

    //
    // Implements the TrcRobot.RobotCommand interface.
    //

    @Override
    public boolean cmdPeriodic(double elapsedTime)
    {
        boolean done = !sm.isEnabled();
        //
        // Print debug info.
        //
        State state = sm.getState();
        robot.dashboard.displayPrintf(1, "State: %s", state != null? sm.getState().toString(): "Disabled");

        if (sm.isReady())
        {
            state = sm.getState();

            switch (state)
            {
                case DO_DELAY:
                    //
                    // Do delay if any.
                    //
                    if (delay == 0.0)
                    {
                        sm.setState(State.ALIGN_WITH_TARGET);
                    }
                    else
                    {
                        timer.set(delay, event);
                        sm.waitForSingleEvent(event, State.ALIGN_WITH_TARGET);
                    }
                    break;
                    
                case ALIGN_WITH_TARGET:
                    //
                    // Position robot at target.
                	boolean aligned = false;
                    //
                	while(!aligned)
            		{
	                	double pos = robot.pixyVision.getTargetPosition();
	                    robot.pidDrive.setTarget(xDistance, yDistance, heading, false, event);
            		}
                	
                    robot.pidDrive.setTarget(xDistance, yDistance, heading, false, event);
                    sm.waitForSingleEvent(event, State.DEPLOY_GEAR);
                    break;
                    
                case DEPLOY_GEAR:
                    //
                    // Place gear on axle.
                    //
                    robot.mailbox.extend(extendTime, event);
                    sm.waitForSingleEvent(event, State.BACKUP);
                    break;
                    
                case BACKUP:
                    //
                    // Backup from axle.
                    //
                    robot.pidDrive.setTarget(0.0, 12.0, 0.0, false, event);
                    sm.waitForSingleEvent(event, State.DONE);
                    break;

                case DONE:
                default:
                    //
                    // We are done.
                    //
                    done = true;
                    sm.stop();
                    break;
            }
            robot.traceStateInfo(elapsedTime, state.toString());
        }

        if (robot.pidDrive.isActive())
        {
            if (xDistance != 0.0)
            {
                robot.encoderXPidCtrl.printPidInfo(robot.tracer);
            }

            if (yDistance != 0.0)
            {
                robot.encoderYPidCtrl.printPidInfo(robot.tracer);
            }

            robot.gyroTurnPidCtrl.printPidInfo(robot.tracer);
        }

        return done;
    }   //cmdPeriodic

}   //class CmdPidDrive

@@ -1644,39 +1644,14 @@
 ble 
-xDistance;%0A    private double y
+backup
 Dist
@@ -1861,27 +1861,14 @@
 ble 
-xDistance, double y
+backup
 Dist
@@ -2006,57 +2006,31 @@
 his.
-xDistance = xDistance;%0A        this.yDistance = y
+backupDistance = backup
 Dist
@@ -3365,16 +3365,36 @@
 target.%0A
+                %09//%0A
         
@@ -3427,39 +3427,16 @@
  false;%0A
-                    //%0A
         
@@ -3549,224 +3549,41 @@
 ();%0A
-%09                    robot.pidDrive.setTarget(xDistance, yDistance, heading, false, event);%0A            %09%09%7D%0A                %09%0A                    robot.pidDrive.setTarget(xDistance, yDistance, heading, false, event);
+            %09%09%7D%0A                %09
 %0A   
@@ -4162,12 +4162,22 @@
 .0, 
-12.0
+backupDistance
 , 0.
@@ -4603,388 +4603,8 @@
  %7D%0A%0A
-        if (robot.pidDrive.isActive())%0A        %7B%0A            if (xDistance != 0.0)%0A            %7B%0A                robot.encoderXPidCtrl.printPidInfo(robot.tracer);%0A            %7D%0A%0A            if (yDistance != 0.0)%0A            %7B%0A                robot.encoderYPidCtrl.printPidInfo(robot.tracer);%0A            %7D%0A%0A            robot.gyroTurnPidCtrl.printPidInfo(robot.tracer);%0A        %7D%0A%0A
     

Add backup parameter