/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       C:\Users\knsv_.KANDULA-3                                  */
/*    Created:      Sat Jul 31 2021                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// left_front           motor         1
// left_back            motor         2
// right_front          motor         3
// right_back           motor         4
// Controller1          controller
// Inertial20           inertial      20
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include "move.h"
#include "turn.h"

using namespace vex;
//global competition instance
competition Competition;

int aside = -1;
int auton = 2;

/****************************************************************************************************************/
void set_auton() {
  if (Brain.Screen.xPosition() < 120 || Brain.Screen.xPosition() > 360) {
    Brain.Screen.setPenWidth(4);
    Brain.Screen.setFillColor(transparent);
    Brain.Screen.setPenColor(black);
    Brain.Screen.drawRectangle(182-180*aside, 2+60*auton, 114, 54);
    Brain.Screen.setPenColor(white);
    if (Brain.Screen.xPosition() < 120)
      aside = 1;
    else
      aside = -1;
    if (Brain.Screen.yPosition() < 60)
      auton = 0;
    else if (Brain.Screen.yPosition() < 120)
      auton = 1;
    else if (Brain.Screen.yPosition() < 180)
      auton = 2;
    else
      auton = 3;
    Brain.Screen.drawRectangle(182-180*aside, 2+60*auton, 114, 54);
  }
}
/****************************************************************************************************************/

void pre_auton(void)
{
 vexcodeInit();

  left_front.setStopping(hold);
  left_front.setVelocity(0, percent);
  left_front.setMaxTorque(100, percent);
  left_back.setStopping(hold);
  left_back.setVelocity(0, percent);
  left_back.setMaxTorque(100, percent);
  right_front.setStopping(hold);
  right_front.setVelocity(0, percent);
  right_front.setMaxTorque(100, percent);
  right_back.setStopping(hold);
  right_back.setVelocity(0, percent);
  right_back.setMaxTorque(100, percent);
}

void autonomous(void)
{
  move::moveBot(10,20);
}

void usercontrol(void)
{
  while(true)
  {

    //Program for setting axis postition relative to controller and wheel
    left_front.setVelocity(Controller1.Axis3.position(percent), percent);
    left_back.setVelocity(Controller1.Axis3.position(percent), percent);
    right_front.setVelocity(Controller1.Axis2.position(percent), percent);
    right_back.setVelocity(Controller1.Axis2.position(percent), percent);

    //code here
  }
}

int main()
{
  vexcodeInit();
 // Set up callbacks for autonomous and driver control periods.
 Competition.autonomous(autonomous);
 Competition.drivercontrol(usercontrol);

 // Run the pre-autonomous function.
 pre_auton();

 while (true) {
   wait(100, msec);
 }
}