#include <stdio.h>
#include "BuiltIns.h"
#include "API.h"
long left_encoder = 0;
long right_encoder = 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

/*******************************************************************
 * 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)
{
  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)
{
}

void func3()
{
  long counter, shifter=-1;

  for(counter=9000; counter--; )
   {
	shifter<<1;
   }
  return;
}

void func2()
{
  func3();
  return;
}

void func1()
{
  func2();
  return;
}


/*******************************************************************
 * 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)
{
  long timeold, timenew;
  unsigned char p1_sw_top;

  timeold=GetMsClock();
  while (1)
  {
    p1_sw_top = GetOIDInput(1,1);
    right_encoder = GetQuadEncoder (right_encoder_port_a, right_encoder_port_b);
    left_encoder = GetQuadEncoder (left_encoder_port_a, left_encoder_port_b);

    Arcade2 (1, 1, 1, 2, 2, 1, 0, 0);

    timenew=GetMsClock();
    printf("Time = %ld  ", timenew);
    if( timenew-timeold > 55 )
      {
	printf("  %ld ******************************************************\r",
	       timenew-timeold);
      } else
      {
		printf("\r");
      }
    timeold = timenew;
    func1();
  }
}

/*******************************************************************
 * 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)
{
}