#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorLeft,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorFork,     tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    leftflap3,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    rightflap4,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    forkL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    forkR8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\gyro.h"
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\Encoders12.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 50.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 50.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
        if (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(driveErr) > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 680) // Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else if(SensorValue[HTMAG] < 640)// Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else
    {
      servoTarget[Mag1] = 127;
    }
}

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[leftflap3] = 17;
  servoTarget[rightflap4] = 252;
  servoTarget[forkL7] = 83;
  servoTarget[forkR8] = 83;
  servoTarget[MagR5] = 0;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.

    while (true)
    {
        GyroTask(g_Gyro);
        motor[motorLift] = 256;
        processMagnet();

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap3] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap3] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        motor[motorLift] = 0;
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

while (true)
    {
        GyroTask(g_Gyro);
        motor[motorLift] = 256;
        processMagnet();

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          motorA,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorFork,     tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftflap2,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightflap3,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    MagG4,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    forkL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    forkR8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\gyro.h"
#include "C:\Users\Matthew\Documents\Robotics\Code\2011-2012\Encoders12.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 50.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 50.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
        if (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(driveErr) > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[leftflap2] = 17;
  servoTarget[rightflap3] = 252;
  servoTarget[MagG4] = 175;
  servoTarget[MagR5] = 60;
  servoTarget[forkL7] = 83;
  servoTarget[forkR8] = 83;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.

    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap2] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap2] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************
            case 0:
                // step 0: go forward.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: Left Flap Out.
                servoTarget[leftflap2] = 135;
                SetDriveTarget(72.0);
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 4:
                // step 2: Left Flap In
                servoTarget[leftflap2] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 5:
                // step 3: wait for drive to complete.
                if (time1[T1] > 500)
                {
                    step++;
                }
                break;

        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }

#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/80.0)     //motor:wheel=40:80 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (0.005807764)

#if 0
/// Copyright (c) Titan Robotics Club. All rights reserved.
///
/// <module name="gyro.h" />
///
/// <summary>
///     This module contains the library functions for the gyro sensor.
/// </summary>
///
/// <remarks>
///     Environment: RobotC for Lego Mindstorms NXT.
/// </remarks>
#endif

#ifndef _GYRO_H
#define _GYRO_H

#include "C:\Projects\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"

#pragma systemFile

//
// Constants.
//
#define GYROF_USER_MASK         0x00ff
#define GYROF_INVERSE           0x0001
#ifdef HTSMUX_STATUS
  #define GYROF_HTSMUX          0x0080
#endif

#define GYRO_NUM_CAL_SAMPLES    50
#define GYRO_CAL_INTERVAL       10

//
// Macros
//
#define __in
#define __out
#define __inout
#define DEADBAND(n,t)           ((abs(n) > (t))? (n): 0)
#define GyroGetTurnRate(p)      (p.turnRate)
#define GyroGetHeading(p)       (p.heading)

//
// Type definitions.
//
typedef struct
{
    int   sensorID;
    int   gyroFlags;
    int   zeroOffset;
    int   deadBand;
    long  timestamp;
    int   turnRate;
    float heading;
} GYRO;

/**
*  This function calibrates the gyro for zero offset and deadband.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param numSamples Specifies the number of calibration samples.
*  @param calInterval Specifies the calibration interval in msec.
*/
void
GyroCal(
    __out GYRO &gyro,
    __in  int numSamples,
    __in  int calInterval
    )
{
    int i;
    int turnRate;
    int min, max;

    gyro.zeroOffset = 0;
    gyro.deadBand = 0;
    min = 1023;
    max = 0;

    for (i = 0; i < numSamples; i++)
    {
#ifdef HTSMUX_STATUS
        turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
        turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
        gyro.zeroOffset += turnRate;

        if (turnRate < min)
        {
            min = turnRate;
        }
        else if (turnRate > max)
        {
            max = turnRate;
        }

        wait1Msec(calInterval);
    }

    gyro.zeroOffset /= numSamples;
    gyro.deadBand = max - min;

    return;
}   //GyroCal

/**
*  This function performs the gyro task where it integrates the turn rate
*  into a heading value.
*
*  @param gyro Points to the GYRO structure.
*/
void
GyroTask(
    __inout GYRO &gyro
    )
{
    long currTime;

    currTime = nPgmTime;
#ifdef HTSMUX_STATUS
    gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
                        HTGYROreadRot((tMUXSensor)gyro.sensorID):
                        HTGYROreadRot((tSensors)gyro.sensorID);
#else
    gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
#endif
    gyro.turnRate -= gyro.zeroOffset;
    gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
    if (gyro.gyroFlags & GYROF_INVERSE)
    {
        gyro.turnRate = -gyro.turnRate;
    }
    gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
    gyro.timestamp = currTime;

    return;
}   //GyroTask

/**
*  This function resets the gyro heading.
*
*  @param gyro Points to the GYRO structure to be reset.
*/
void
GyroReset(
    __out GYRO &gyro
    )
{
    GyroTask(gyro);
    gyro.heading = 0;
    return;
}   //GyroReset

/**
*  This function initializes the gyro sensor.
*
*  @param gyro Points to the GYRO structure to be initialized.
*  @param sensorID Specifies the ID of the gyro sensor.
*  @param gyroFlags Specifies the gyro flags.
*/
void
GyroInit(
    __out GYRO &gyro,
    __in  int sensorID,
    __in  int gyroFlags
    )
{
    gyro.sensorID = sensorID;
    gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
#ifdef HTSMUX_STATUS
    if (gyro.gyroFlags & GYROF_HTSMUX)
    {
        HTGYROstartCal((tMUXSensor)sensorID);
    }
    else
    {
        HTGYROstartCal((tSensors)sensorID);
    }
#else
    HTGYROstartCal((tSensors)sensorID);
#endif
    GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
    gyro.timestamp = nPgmTime;
    GyroReset(gyro);

    return;
}   //GyroInit
#endif  //ifndef _GYRO_H

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                    HiTechnic Servo/Motor Controller Device Driver  - UPDATED 1/08/2009//
//
// With the TETRIX system, the PC Controller Station sends messages over Bluetooth to the NXT containing
// current settings of a PC joystick. The joystick settings arrive using the standard NXT BLuetooth
// "message mailbox" facility.
//
// This is a short ROBOTC program to extract the joystick data from a mailbox message and format it
// into data structure that can be easily acccessed by end user programs.
//
// The driver resides in a separate file that can be simply added to any user program with a
// "#include" preprocessor directive. End users should not have to modify this program and can use
// it as is.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (defined(NXT) || defined(TETRIX)) && (_TARGET == "Robot")
   #pragma autoStartTasks        // Automatically start this task when the main user program starts.
#elif (defined(VEX2) || defined(NXT) || defined(TETRIX)) && (_TARGET == "VirtWorld")
  //Don't auto start tasks
#else
   #error "This driver is not supported on this platform."
#endif

#pragma systemFile

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    Joystick Information Structure
//
//
// Structure containing info from Joystick.
//
// "short" variables are used to prevent conversion errors. For example, negating a byte variable with
// value -128 results in -128 because -128 does not fit in a byte!
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (_TARGET == "Robot")
typedef struct
{
   bool    UserMode;          // Autonomous or Telep-Operated mode
   bool    StopPgm;           // Stop program

   short   joy1_x1;           // -128 to +127
   short   joy1_y1;           // -128 to +127
   short   joy1_x2;           // -128 to +127
   short   joy1_y2;           // -128 to +127
   short   joy1_Buttons;      // Bit map for 12-buttons
   short   joy1_TopHat;       // value -1 = not pressed, otherwise 0 to 7 for selected "octant".

   short   joy2_x1;           // -128 to +127
   short   joy2_y1;           // -128 to +127
   short   joy2_x2;           // -128 to +127
   short   joy2_y2;           // -128 to +127
   short   joy2_Buttons;      // Bit map for 12-buttons
   short   joy2_TopHat;       // value -1 = not pressed, otherwise 0 to 7 for selected "octant".
} TJoystick;
TJoystick joystick;      // User defined variable access

#else
TPCJoystick joystick;
#endif


#if defined(hasJoystickMessageOpcodes)
  intrinsic void getJoystickSettings(TJoystick &joystick)
                         asm(opcdSystemFunctions, byte(sysFuncGetJoysticks),
                               variableRefRAM(joystick));
#endif


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// "Macro" to get a non-volatile copy of the last joystick settings so
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

#if (_TARGET == "Robot")
#define getJoystickSettings(joystick)    memcpy(joystick, joystickCopy, sizeof(joystick))

bool joy1Btn(int btn)
{   return ((joystick.joy1_Buttons & (1 << (btn - 1))) != 0);  }
bool joy2Btn(int btn)
{   return ((joystick.joy2_Buttons & (1 << (btn - 1))) != 0);  }

#else

#define getJoystickSettings getPCJoystickSettings
bool joy1Btn(int btn)
{   return ((joystick.joy1_Buttons & (1 << (btn - 1))) != 0);  }
#endif


// Code Below Does Not Apply to Virtual/Emulator Robots
#if (defined(NXT) || defined(TETRIX)) && (_TARGET == "Robot")
const TMailboxIDs kJoystickQueueID = mailbox1;
TJoystick joystickCopy;  // Internal buffer to hold the last received message from the PC. Do not use

long ntotalMessageCount = 0;

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Receive Messages Task
//
// Dedicated task that continuously polls for a Bluetooth message from the PC containing the joystick
// values. Operaton of this task is nearly transparent to the end user as the earlier "#pragma autoStartTasks"
// statement above will cause it to start running as soon as the user program is started.
//
// The task is very simple. It's an endless loop that continuously looks for a Bluetooth mailbox
// message from the PC. When one is found, it reformats and copies the contents into the internal
// "joystickCopy" buffer.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
#if (_TARGET == "Robot")
bool bDisconnected = false;
bool bOverrideJoystickDisabling = false;
long nNoMessageCounterLimit = 750;
long nNoMessageCounter = 0;
task readMsgFromPC()
{
   bool bMsgFound;

  TFileIOResult nBTCmdRdErrorStatus;
   const int kMaxSizeOfMessage = 18;
   sbyte tempBuffer[kMaxSizeOfMessage];

   // Initialize setting to default values in case communications with PC is broken.

   //joystickCopy.TeamColor = false;
   joystickCopy.UserMode  = false;
   joystickCopy.StopPgm   = true;

   joystickCopy.joy1_x1 = 0;
   joystickCopy.joy1_y1 = 0;
   joystickCopy.joy1_x2 = 0;
   joystickCopy.joy1_y2 = 0;
   joystickCopy.joy1_Buttons = 0;
   joystickCopy.joy1_TopHat = -1;

   joystickCopy.joy2_x1 = 0;
   joystickCopy.joy2_y1 = 0;
   joystickCopy.joy2_x2 = 0;
   joystickCopy.joy2_y2 = 0;
   joystickCopy.joy2_Buttons = 0;
   joystickCopy.joy2_TopHat = -1;

   bool bTempUserMode,bTempStopPgm;

   while (true)
   {
      // Check to see if a message is available.
      bMsgFound = false;
      bDisconnected = false;
      while (true)
      {
       //
         // There may be more than one message in the queue. We want to get to the last received
         // message and discard the earlier "stale" messages. This loop simply discards all but
         // the last message.
         //
         int nSizeOfMessage;


         nSizeOfMessage = cCmdMessageGetSize(kJoystickQueueID);

         if (nSizeOfMessage <= 0)
         {
            if (!bMsgFound)
            {
               if(nNoMessageCounter > nNoMessageCounterLimit)
               {
                 if(!bOverrideJoystickDisabling)
                 {
                   bTempUserMode = joystickCopy.UserMode;
                   bTempStopPgm = joystickCopy.StopPgm;

                   memset(joystickCopy, 0, sizeof(joystickCopy));

                   joystickCopy.UserMode = bTempUserMode;
                   joystickCopy.StopPgm = bTempStopPgm;
                   joystickCopy.joy1_TopHat = -1;
                   joystickCopy.joy2_TopHat = -1;
                 }
                 bDisconnected = true;
              }
              wait1Msec(4);    // Give other tasks a chance to run
              nNoMessageCounter++;
              continue;        // No message this time. Loop again
            }
            else
            {
              bDisconnected = false;
              nNoMessageCounter = 0;
              break;
           }
            //
            // No more messages available and at least one message found. This is not essential but
            // useful to ensure that we're working with the latest message. We simply discard earlier
            // messages. This is useful because there could be many messages queued and we don't
            // want to work with stale data.
            //
         }

        if (nSizeOfMessage > sizeof(tempBuffer))
          nSizeOfMessage = sizeof(tempBuffer);
        nBTCmdRdErrorStatus = cCmdMessageRead((ubyte)tempBuffer, nSizeOfMessage, kJoystickQueueID);
        nBTCmdRdErrorStatus = nBTCmdRdErrorStatus; //Get rid of info message
        //
        // Repeat loop until there are no more messages in the queue. We only want to process the
        // last message in the queue.
        //
        bMsgFound = true;
      }

     // Once we've reached here, a valid message is available

      hogCPU();   // grab CPU for duration of critical section

      ++ntotalMessageCount;

      joystickCopy.UserMode           = tempBuffer[1];
      joystickCopy.StopPgm            = tempBuffer[2];

      joystickCopy.joy1_x1            = tempBuffer[3];
      joystickCopy.joy1_y1            = tempBuffer[4];
      joystickCopy.joy1_x2            = tempBuffer[5];
      joystickCopy.joy1_y2            = tempBuffer[6];
      joystickCopy.joy1_Buttons       = (tempBuffer[7] & 0x00FF) | (tempBuffer[8] << 8);
      joystickCopy.joy1_TopHat        = tempBuffer[9];

      joystickCopy.joy2_x1            = tempBuffer[10];
     joystickCopy.joy2_y1            = tempBuffer[11];
     joystickCopy.joy2_x2            = tempBuffer[12];
     joystickCopy.joy2_y2            = tempBuffer[13];
     joystickCopy.joy2_Buttons       = (tempBuffer[14] & 0x00FF) | (tempBuffer[15] << 8);
      joystickCopy.joy2_TopHat        = tempBuffer[16];

     joystickCopy.joy1_y1            = -joystickCopy.joy1_y1; // Negate to "natural" position
     joystickCopy.joy1_y2            = -joystickCopy.joy1_y2; // Negate to "natural" position

     joystickCopy.joy2_y1            = -joystickCopy.joy2_y1; // Negate to "natural" position
     joystickCopy.joy2_y2            = -joystickCopy.joy2_y2; // Negate to "natural" position


     releaseCPU(); // end of critical section
   }
}
#endif

///////////////////////////////////////////////////////////////////////////////////////////
//
//                                        displayDiagnostics
//
// THis task will display diagnostic information about a TETRIX robot on the NXT LCD.
//
// If you want to use the LCD for your own debugging use, call the function
// "disableDiagnosticsDisplay()
//
///////////////////////////////////////////////////////////////////////////////////////////


bool bDisplayDiagnostics = false;  // Set to false in user program to disable diagnostic display

void getUserControlProgram(string& sFileName);

#if defined(TETRIX) && (_TARGET == "Robot")

void disableDiagnosticsDisplay()
{
  bDisplayDiagnostics = false;   // Disable diagnostic display
}

task displayDiagnostics()
{
  string sFileName;
  bNxtLCDStatusDisplay = true;
  getUserControlProgram(sFileName);

  while (true)
   {
      if (bDisplayDiagnostics)
      {
        nxtDisplayTextLine(6, "Teleop FileName:");
        nxtDisplayTextLine(7, sFileName);

        getJoystickSettings(joystick);                   //Update variables with current joystick values

         if (joystick.StopPgm)
           nxtDisplayCenteredTextLine(1, "Wait for Start");
         else if (joystick.UserMode)
            nxtDisplayCenteredTextLine(1, "TeleOp Running");
         else
            nxtDisplayCenteredTextLine(1, "Auton Running");

         if ( externalBatteryAvg < 0)
            nxtDisplayTextLine(3, "Ext Batt: OFF");       //External battery is off or not connected
         else
            nxtDisplayTextLine(3, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);

         nxtDisplayTextLine(4, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000);   // Display NXT Battery Voltage

         nxtDisplayTextLine(5, "FMS Msgs: %d", ntotalMessageCount);   // Display Count of FMS messages
      }

      wait1Msec(200);
  }
}

///////////////////////////////////////////////////////////////////////////////////////////
//
//                                        getUserControlProgram
//
// This function returns the name of the TETRIX User Control program. It reads the file
// "FTCConfig.txt" and builds the name of the file from the contents.
//
// Note that the filename includes the ".rxe" (robot executable file) file extension.
//
///////////////////////////////////////////////////////////////////////////////////////////
#endif
const string kConfigName = "FTCConfig.txt";

void getUserControlProgram(string& sFileName)
{
  byte   nParmToReadByte[2];
  int    nFileSize;
   TFileIOResult nIoResult;
   TFileHandle hFileHandle;

  sFileName = "";
  nParmToReadByte[1] = 0;
  hFileHandle = 0;
  OpenRead(hFileHandle, nIoResult, kConfigName, nFileSize);
  if (nIoResult == ioRsltSuccess)
  {
    for (int index = 0; index < nFileSize; ++index)
    {
      ReadByte(hFileHandle, nIoResult,  nParmToReadByte[0]);
      strcat(sFileName, nParmToReadByte);
    }

    //
    // Delete the ".rxe" file extension
    //
    int nFileExtPosition;

    nFileExtPosition = strlen(sFileName) - 4;
    if (nFileExtPosition > 0)
      StringDelete(sFileName, nFileExtPosition, 4);
  }
  Close(hFileHandle, nIoResult);
  return;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    waitForStart
//
// Wait for the start of either the autonomous or tele-op phase. User program is running on the NXT
// but the phase has not yet started. The FMS (Field Management System) is continually (every 50 msec)
// sending information to the NXT. This program loops getting the latest value of joystick settings.
// When it finds that the FMS has started the  phase, it immediately returns.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void waitForStart()
{
  while (true)
  {
    getJoystickSettings(joystick);
    if (!joystick.StopPgm)
      break;
  }
  return;
}



#endif