#include <stdio.h>
#include "BuiltIns.h"
#include "API.h"
#include "position.h"
#define pressure (Getdigitalinput (17) == 1)
int leftpower, rightpower;
long left_encoder = 0;
long right_encoder = 0;
long left_dist = 0;
long right_dist = 0;
#define left_encoder_port_a 2
#define left_encoder_port_b 8
#define right_encoder_port_a 1
#define right_encoder_port_b 7
#define PRESSED		1	/* Button is pressed  */
#define NOT_PRESSED	0	/* Button has not been pressed */

/*******************************************************************
 * This function must be here for a competition project. It is
 * automatically referenced by WPILib at startup and run. At that
 * point the SetCompetitionMode function sets the competition
 * mode. Basically, a mode of 0 is the default (without the
 * IO_Initialization function) and runs a main function only.
 * SetCompetitionMode(1) runs a competition project as shown. */
void IO_Initialization (void)
{
    SetCompetitionMode (1);
}

/*******************************************************************
 * Initialize is run immediately when the robot is powered on
 * regardless of the field mode.
 */
void Initialize(void)
{

    InitPressureSwitch(15, 8); //connect pneumatic pressure switch to digital input 15, and power to air compressor to relay 08

	SetDirection(14, 1);
	SetDirection(15, 1);
	SetDirection(16, 1);
	SetDirection(17, 1);

	PresetQuadEncoder (left_encoder_port_a, left_encoder_port_b, 0);
        PresetQuadEncoder (right_encoder_port_a, right_encoder_port_b, 0);
        StartQuadEncoder (right_encoder_port_a, right_encoder_port_b, 0);
        StartQuadEncoder (left_encoder_port_a, left_encoder_port_b, 1);

	


}

/*******************************************************************
 * Autonomous is run as soon as the field controls enable the
 * robot. At the end of the autonomous period, the Autonomous
 * function will end (note: even if it is in an infinite loop
 * as in the example, it will be stopped).
 */
void Autonomous (void)
{


    int state;
    long angle;
    int intercept;
    int robot_pos;
    int left_power = 0;
    int right_power = 0;

    while (1)

    {
	left_dist = 39 * left_encoder / 128;
        right_dist = 39 * right_encoder / 128;

        state = get_ball_position();
	angle = get_angle();
	robot_pos = get_robot_pos();
	

move(120,160);

}




/*******************************************************************
 * The OperatorControl function will be called when the field
 * switches to operator mode. If the field ever switches back
 * to autonomous, then OperatorControl will automatically exit
 * and the program will transfer control to the Autonomous
 * function.
 */
void OperatorControl (void)
{
#define JOY1 2
#define JOY1NEW 3
#define JOY2 4
#define JOY2NEW 5
#define ONEJOYSTICK (char)0x1

    unsigned char p1_sw_top;
    static char joystate=JOY1,
                         button_old=0;
    while (1)
    {
        p1_sw_top = GetOIDInput(1,1);

        switch (joystate)
        {
        case JOY1:
            if ((debounce_button( &button_old, p1_sw_top )) == 1)
                joystate = JOY2NEW;
            break;

        case JOY2NEW:
            if ((debounce_button( &button_old, p1_sw_top )) == 0)
                joystate = JOY2;
            break;

        case JOY2:
            if ((debounce_button( &button_old, p1_sw_top )) == 1)
                joystate = JOY1NEW;
            break;

        case JOY1NEW:
            if ((debounce_button( &button_old, p1_sw_top )) == 0)
                joystate = JOY1;
            break;

        }
        if (joystate & 0x2 )
        {// one joystick
            Arcade2 (1, 1, 1, 2, 2, 1, 0, 0);
        }
        else
        {
            Tank2 (1,1,2,1,2,1,0,0);
        }

    }
}

    /******************************************************************************
    * FUNCTION NAME: debounce_button
    * PURPOSE:       Checks if button in question has been changed from unpressed
    *                to pressed in the last cycle.
    * CALLED FROM:   this file
    * AUTHOR:		 John-Nicholas
    * ARGUMENTS:
    *     Argument       Type             IO   Description
    *     --------       -------------    --   -----------
    *     button         char              I   joystick button
    *     *last_button   pointer           O   last button state
    * RETURNS:       char
    *****************************************************************************/
    char debounce_button (char *last_button, char button)
    {
        if (button != *last_button && *last_button == PRESSED)

        {
            *last_button = button;
            return PRESSED;
        }

        else

        {
            *last_button = button;
            return NOT_PRESSED;
        }
    }

    /*******************************************************************
     * the main program is not used, but needs to be here to
     * statisfy a reference to it. This requirement will probably
     * go away in the next version of WPILib.
     */
    void main (void)
    {
    }