package ca.teamdave.letterman.auto.modes;

import ca.teamdave.letterman.auto.commands.drive.DriveToPoint;
import ca.teamdave.letterman.auto.commands.drive.TurnToHeading;
import ca.teamdave.letterman.auto.commands.drive.WaitForDriveStopped;
import ca.teamdave.letterman.auto.commands.meta.Latch;
import ca.teamdave.letterman.config.command.WaitForDriveStoppedConfig;
import ca.teamdave.letterman.descriptors.RobotPose;
import ca.teamdave.letterman.descriptors.RobotPosition;
import ca.teamdave.letterman.auto.commands.*;
import ca.teamdave.letterman.auto.commands.dummy.DummyShoot;
import ca.teamdave.letterman.auto.commands.meta.Pause;
import ca.teamdave.letterman.auto.commands.meta.Series;
import ca.teamdave.letterman.config.command.DriveToPointConfig;
import ca.teamdave.letterman.config.command.TurnToHeadingConfig;
import ca.teamdave.letterman.config.control.PidControllerConfig;
import ca.teamdave.letterman.robotcomponents.DriveBase;

/**
 * Auto mode that does the driving pattern for scoring two balls
 */
public class ScoreTwoDriving implements AutoMode {

    private final DriveBase mDriveBase;

    public ScoreTwoDriving(DriveBase driveBase) {
        mDriveBase = driveBase;
    }

    public RobotPose getInitialPose() {
        return new RobotPose(new RobotPosition(0, 0), 0);
    }

    public AutoCommand getRootCommand() {

        PidControllerConfig turnPidConfig = new PidControllerConfig(0.01, 0.01, 0);
        PidControllerConfig drivePidConfig = new PidControllerConfig(0.1, 0, 0);


        RobotPosition shootPosition = new RobotPosition(8, 0);
        RobotPosition pickupPosition = new RobotPosition(-1, 0);

        DriveToPointConfig driveOut = new DriveToPointConfig(
                shootPosition,
                1,
                0.5,
                turnPidConfig,
                drivePidConfig);
        DriveToPointConfig driveBackToBall = new DriveToPointConfig(
                pickupPosition,
                1,
                0.25,
                turnPidConfig,
                drivePidConfig);

        return new Series(new AutoCommand[] {
                // TODO: drive this construction nicely from somewhere
                new DriveToPoint(driveOut, mDriveBase),
                new Latch(new AutoCommand[]{
                        new TurnToHeading(
                                new TurnToHeadingConfig(20, 2, turnPidConfig),
                                mDriveBase),
                        new WaitForDriveStopped(new WaitForDriveStoppedConfig(0.1, 5), mDriveBase)
                }),
                new DummyShoot(),

                // TODO: run pickup here
                new DriveToPoint(driveBackToBall, mDriveBase),
                new Pause(0.5),
                // TODO: stop running pickup here

                new DriveToPoint(driveOut, mDriveBase),
                new Latch(new AutoCommand[]{
                        new TurnToHeading(
                                new TurnToHeadingConfig(-20, 2, turnPidConfig),
                                mDriveBase),
                        new WaitForDriveStopped(new WaitForDriveStoppedConfig(0.1, 5), mDriveBase)
                }),
                new DummyShoot()
        });
    }
}

@@ -99,16 +99,76 @@
 oPoint;%0A
+import ca.teamdave.letterman.auto.commands.drive.StopDrive;%0A
 import c
@@ -994,16 +994,47 @@
 iveBase;
+%0Aimport org.json.me.JSONObject;
 %0A%0A/**%0A *
@@ -1099,16 +1099,44 @@
 alls%0A */
+                           %60
 %0Apublic 
@@ -1219,16 +1219,53 @@
 iveBase;
+%0A    private final JSONObject mConfg;
 %0A%0A    pu
@@ -1304,16 +1304,34 @@
 riveBase
+, JSONObject confg
 ) %7B%0A    
@@ -1354,24 +1354,48 @@
  driveBase;%0A
+        mConfg = confg;%0A
     %7D%0A%0A    p
@@ -2733,32 +2733,75 @@
             %7D),%0A
+                new StopDrive(mDriveBase),%0A
                 
@@ -3386,32 +3386,75 @@
             %7D),%0A
+                new StopDrive(mDriveBase),%0A
                 

Stop drive after turning in two ball auto