#include "robot-config.h"
//Creates a competition object that allows access to Competition methods.
vex::competition    Competition;
using namespace vex;


//Pre Auton
void pre_auton( void )
{
    //reset all encoder value for motors
    Puncher.resetRotation();
    Intake.resetRotation();
    RDriveF.resetRotation();
    RDriveB.resetRotation();
    LDriveF.resetRotation();
    LDriveB.resetRotation();
    Arm.resetRotation();
    Angler.resetRotation();
}

//bool canShoot = true;

//Ease of Use Tasks
void motorSpin(vex::motor aMotor, int aRpm)
{
    aMotor.spin(directionType::fwd, aRpm, vex::velocityUnits::rpm);
}

void motorRotate(vex::motor aMotor, int aRpm, int aRev, bool aWait)
{
    aMotor.rotateTo(aRev, vex::rotationUnits::rev, aRpm, vex::velocityUnits::rpm, false);
}

//Robot Tasks

void singleShot()
{
    Puncher.rotateFor(1, vex::rotationUnits::rev, 200, vex::velocityUnits::rpm, true);
    //Puncher.stop(brakeType::coast);
}

void setAngler(int aAngle)
{
    //canShoot = false;
    if(aAngle > AnglerPot.value(vex::analogUnits::range12bit))
    {
        while(aAngle > AnglerPot.value(vex::analogUnits::range12bit))
        {
            motorSpin(Angler, 200);
            vex::task::sleep(10);
        }
        Angler.stop(brakeType::hold);
    }
    else if(aAngle < AnglerPot.value(vex::analogUnits::range12bit))
    {
        while(aAngle < AnglerPot.value(vex::analogUnits::range12bit))
        {
            motorSpin(Angler, -200);
            vex::task::sleep(10);
        }
        Angler.stop(brakeType::hold);
    }
    //canShoot = true;
}

//Auton
void autonomous( void )
{

}


//Driver Control Tasks

//Angler

int anglerCounter = 0;
int anglerTask()
{
    while (Competition.isDriverControl() && Competition.isEnabled())
    {
        if(Controller1.ButtonR2.pressing())
        {
            while(Controller1.ButtonR2.pressing())
            {
                vex::task::sleep(10);
            }
            /*switch(anglerCounter)
            {
                case 0:
                    setAngler(1312);
                    anglerCounter++;
                    break;
                case 1:
                    setAngler(1450);
                    anglerCounter = 0;
                    break;
            }*/
            setAngler(1312);
        }
        vex::task::sleep(50);
    }
    return 0;
}

//Shoot

int shootTask()
{
    while (Competition.isDriverControl() && Competition.isEnabled())
    {
        if(Controller1.ButtonR1.pressing())
        {
            while(Controller1.ButtonR1.pressing())
            {
                vex::task::sleep(10);
            }
            singleShot();
        }
        vex::task::sleep(50);
    }
    return 0;
}

//Driver Control

void usercontrol( void )
{
    while (Competition.isDriverControl() && Competition.isEnabled())
    {
        vex::task angler = vex::task(anglerTask);
        vex::task shoot = vex::task(shootTask);
        vex::task::sleep(20);
    }
}

//
// Main will set up the competition functions and callbacks.
//
int main() {

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

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

    //Prevent main from exiting with an infinite loop.
    while(1) {
      vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
    }

}