/*******************************************************************************
 * @mainpage
 * This is a template for the vexuser.c file.
 *
 * This program is based on a template by James Pearman in his "C on Vex"
 * library, V 1.00.
 *
 * @author James Pearman
 * @author Liam Bohl <liambohl@gmail.com>
 * @author Faith Bosher <FABosher@gmail.com>
 *
 * @since 2014-12-22
 ******************************************************************************/
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "ch.h"             //ChibiOS programs
#include "hal.h"            //hardware abstraction layer header
#include "vex.h"            //vex library header
#include "apollo.h"         //includes the apollo debug window in screen
#include "smartmotor.h"     //smart motors
#include "vexime.h"

#include "clicks.c"         //register and check when a button is clicked

#include "vexuser.h"        //function declarations

//Motor Names
#define FRONT_LEFT          kVexMotor_2     //for example only
#define FRONT_RIGHT         kVexMotor_4
#define BACK_RIGHT          kVexMotor_5
#define BACK_LEFT           kVexMotor_3



// Digi IO configuration
static  vexDigiCfg  dConfig[kVexDigital_Num] = {
        { kVexDigital_1,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_2,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_3,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_4,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_5,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_6,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_7,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_8,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_9,    kVexSensorDigitalInput,     kVexConfigInput,        0 },
        { kVexDigital_10,   kVexSensorDigitalInput,     kVexConfigInput,        0 },
};

//Motor configuration
static  vexMotorCfg mConfig[kVexMotorNum] = {

        { kVexMotor_1,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_2,      kVexMotor393S,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_3,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_4,      kVexMotor393T,      kVexMotorNormal,        kVexSensorIME,          kImeChannel_1 },
        { kVexMotor_5,      kVexMotor393T,      kVexMotorNormal,        kVexSensorIME,          kImeChannel_2 },
        { kVexMotor_6,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_7,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_8,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_9,      kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 },
        { kVexMotor_10,     kVexMotor393T,      kVexMotorNormal,        kVexSensorNone,         0 }
};

//Global variables

//Global constants

/**
 * @brief
 * User setup
 * @details
 * The digital and motor ports can (should) be configured here.
 */
void vexUserSetup()
{
    vexDigitalConfigure(dConfig, DIG_CONFIG_SIZE( dConfig));
    vexMotorConfigure(mConfig, MOT_CONFIG_SIZE( mConfig));
}

/**
 * @brief
 * User initialize
 * @details
 * This function is called after all setup is complete and communication has
 * been established with the master processor.
 * Start other tasks and initialize user variables here.
 */
void vexUserInit()
{

}


/**
 * @brief
 * Autonomous
 * @details
 * This thread is started when the autonomous period is started.
 */
msg_t vexAutonomous(void *arg)
{
    (void)arg;

    // Must call this
    vexTaskRegister("auton");

    //Auton code goes here. It is advisable to have the routine written in a separate function.

    return (msg_t)0;
}

/**
 * @brief
 * Driver Control
 * @details
 * This thread is started when the driver control period is started.
 */
msg_t vexOperator(void *arg)
{
    (void)arg;

    // Must call this
    vexTaskRegister("operator");

    while(!chThdShouldTerminate())
    {


        //Driver control code goes here.



        //Don't hog CPU from other tasks
        vexSleep(20);
    }

    return (msg_t)0;
}