/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       C:\Users\fathe                                            */
/*    Created:      Sat Oct 23 2021                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Controller          controller
// LeftBack             motor         1
// LeftFront            motor         2
// RightBack            motor         3
// RightFront           motor         4
// RightForkLift        motor         5
// LeftForkLift         motor         6
// RingIntake           motor         7
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <iostream>
#include <array>

using namespace vex;
using namespace std;

string statuses[4] = {"auto", "drive", "record", "game"}; // Autonomous Test, Driver Test, Autonomous Recorder, ACTUAL GAME (choose autonomous too)
string status;
string buttons[4] = {"(A)", "(B)", "(X)", "(Y)"};
string autons[2] = {"right", "left"};
string auton;

string chooseMode(string desc, array<string> modes, int default_index, double timeout = 5000) { // MAX OF 4 MODES ONLY
  int length = modes.size();
  Controller.lcd.clearScreen();
  Controller.lcd.setCursor(0, 0);
  Controller.lcd.print(desc);

  for (int i=0;i<length;i++) {
    Controller.lcd.newLine();
    Controller.lcd.print(buttons[i] + modes[i]);
  }

  timer _timer = timer::timer();

  while (_timer.time(timeUnits::msec) < timeout) {
    if (Controller.ButtonA.pressing()) return modes[0];
    if (Controller.ButtonB.pressing()) return modes[1];
    if (Controller.ButtonX.pressing() && modes.size() > 2) return modes[2];
    if (Controller.ButtonY.pressing() && modes.size() > 3) return modes[3];
  }
  return modes[default_index];
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  status = chooseMode("Status", statuses, 3);

  printf("Status: %d\n", status);

  if (status == "auto" || status == "game") {
    auton = chooseMode("Autonomous", autons, 0, 3000);

    printf("Autonomous: %d\n", auton);
  }

  return 0;
}