package org.usfirst.frc.team1360.robot.util;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team1360.robot.autonomous.AutonomousGroupBuilder;
import org.usfirst.frc.team1360.robot.autonomous.actions.AutonomousDriveCommand;
import org.usfirst.frc.team1360.robot.autonomous.actions.AutonomousIntakeCommand;
import org.usfirst.frc.team1360.robot.autonomous.actions.AutonomousPivotCommand;
import org.usfirst.frc.team1360.robot.autonomous.actions.AutonomousShooterCommand;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;

public class DriverstationDashboard
{
    private static List<SendableChooser> choices = new ArrayList<>();

    private static HashMap<String, CommandData.DataType> getCommandDataArgs(Command command)
    {
        HashMap<String, CommandData.DataType> data = new HashMap<>();
        if (command instanceof AutonomousDriveCommand)
        {
            data = ((AutonomousDriveCommand) command).getCommandDataArguments();
        }
        else if (command instanceof AutonomousIntakeCommand)
        {
            data = ((AutonomousIntakeCommand) command).getCommandDataArguments();
        }
        else if (command instanceof AutonomousPivotCommand)
        {
            data = ((AutonomousPivotCommand) command).getCommandDataArguments();
        }
        else if (command instanceof AutonomousShooterCommand)
        {
            data = ((AutonomousShooterCommand) command).getCommandDataArguments();
        }
        return data;
    }

    private static CommandData populateCommandData(Command command)
    {
        CommandData output = new CommandData();
        HashMap<String, CommandData.DataType> data = getCommandDataArgs(command);
        for(String s : data.keySet())
        {
            CommandData.DataType dataType = data.get(s);
            switch(dataType)
            {
                case DOUBLE:
                    output.addDouble(s, 0);
                    break;
                case BOOLEAN:
                    output.addBoolean(s, false);
                    break;
                case FLOAT:
                    output.addFloat(s, 0f);
                    break;
            }
        }
        return output;
    }

    private static void addSendables()
    {
        for (int i = 0; i < choices.size(); i++)
        {
            SendableChooser c = choices.get(i);
            SmartDashboard.putData("Autonomous " + i, c);
        }
    }

    private static void initAutonomousActions()
    {
        List<Command[]> sections = new ArrayList<>();


        Command[] section1 =
                {
                    new AutonomousDriveCommand()
                };
        Command[] section2 =
                {

                };
        Command[] section3 =
                {

                };

        sections.add(section1);
        sections.add(section2);
        sections.add(section3);

        for (int i = 0; i < 3; i++)
        {
            SendableChooser s = choices.get(i);
            for (Command c : sections.get(i))
                if (i == 0)
                    s.addDefault(c.getName(), c);
                else
                    s.addObject(c.getName(), c);
        }
    }
    private static HashMap<String, CommandData.DataType> getAllCommandDataArgs()
    {
        AutonomousDriveCommand driveCommand = new AutonomousDriveCommand();
        AutonomousPivotCommand pivotCommand = new AutonomousPivotCommand();
        AutonomousShooterCommand shooterCommand = new AutonomousShooterCommand();
        AutonomousIntakeCommand intakeCommand = new AutonomousIntakeCommand();
        HashMap<String, CommandData.DataType> data = new HashMap<>();

        data.putAll(driveCommand.getCommandDataArguments());
        data.putAll(pivotCommand.getCommandDataArguments());
        data.putAll(shooterCommand.getCommandDataArguments());
        data.putAll(intakeCommand.getCommandDataArguments());
        return data;
    }


    private static void putValues()
    {
        HashMap<String, CommandData.DataType> data = getAllCommandDataArgs();

        for(String s : data.keySet())
        {
            CommandData.DataType dataType = data.get(s);
            switch (dataType)
            {
                case BOOLEAN:
                    SmartDashboard.putBoolean(s, false);
                    break;
                case DOUBLE:
                case FLOAT:
                    SmartDashboard.putNumber(s, 0);
                    break;
            }
        }
    }

    private static HashMap<String,Double> getValues()
    {
        HashMap<String, Double> values = new HashMap<>();

        HashMap<String, CommandData.DataType> data = getAllCommandDataArgs();
        for(String s : data.keySet())
            values.put(s, SmartDashboard.getNumber(s));
        return values;
    }

    public static void init()
    {
        for (int i = 0; i < 3; i++)
        {
            SendableChooser c = new SendableChooser();
            choices.add(c);
        }
        addSendables();
        putValues();
        initAutonomousActions();
    }

    public static Command getAutonomousChoice()
    {
        List<Command> actions = new ArrayList<>();
        for (SendableChooser s : choices)
            actions.add((Command) s.getSelected());
        return new AutonomousGroupBuilder(actions);
    }
}

@@ -39,16 +39,55 @@
 .util;%0A%0A
+import edu.wpi.first.wpilibj.Sendable;%0A
 import e
@@ -4695,30 +4695,19 @@
 tic 
-HashMap%3CString,Double%3E
+CommandData
  get
@@ -4771,33 +4771,79 @@
 ew HashMap%3C%3E();%0A
+        CommandData cdata = new CommandData();
 %0A
-
         HashMap%3C
@@ -5002,29 +5002,543 @@
 ));%0A
-        return values
+%0A        cdata.setDoubles(values);%0A        return cdata;%0A    %7D%0A    private static SendableChooser sendableChooser = new SendableChooser();%0A    public static void initSimpleChooser()%0A    %7B%0A        sendableChooser.addDefault(%22Drive%22, new AutonomousDriveCommand(getValues()));%0A        sendableChooser.addObject(%22Shoot%22, new AutonomousShooterCommand(getValues()));%0A        SmartDashboard.putData(%22SimpleAuto%22, sendableChooser);%0A    %7D%0A%0A    public static Command getSimpleChooser()%0A    %7B%0A        return (Command)sendableChooser.getSelected()
 ;%0A  
@@ -5725,16 +5725,18 @@
         
+//
 addSenda
@@ -5764,24 +5764,24 @@
 lues();%0A
-
         
 initAuto
@@ -5772,16 +5772,18 @@
         
+//
 initAuto

Add a simple auto selector just in case