#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Motor,  motorA,          motorRight,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorLeft,     tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "gyro.h"
#include "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()
{
#if 0
  servoTarget[Mag1] = 127;
  servoTarget[leftflap3] = 17;
  servoTarget[rightflap4] = 252;
  servoTarget[forkL7] = 83;
  servoTarget[forkR8] = 83;
  servoTarget[MagR5] = 0;
#endif
  // 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_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: turn 90 left degrees.
                SetTurnTarget(90.0);
                step++;
                break;

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

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

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

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

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

        }

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