#define CONVEX      1

#ifdef  CONVEX
// CONVEX specific definitions
#include <stdlib.h>

#include "ch.h"         // needs for all ChibiOS programs
#include "hal.h"        // hardware abstraction layer header
#include "vex.h"        // vex library header

#include "robotc_glue.h"        // robot c glue code

#define DRIVE_RIGHT_MOTOR       kVexMotor_1
#define DRIVE_LEFT_MOTOR        kVexMotor_10
#define ARM_MOTOR               kVexMotor_8
#define armPot                  kVexAnalog_1

#else
// ROBOTC specific definitions

#define DRIVE_RIGHT_MOTOR       port1
#define DRIVE_LEFT_MOTOR        port10
#define ARM_MOTOR               port8
#define armPot                  in1
#endif

#define DRIVE_FORWARD_SPEED     100
#define DEFAULT_DRIVE_TIMEOUT   5000

/*-----------------------------------------------------------------------------*/
/*  Glue functions to make code portable                                       */
/*-----------------------------------------------------------------------------*/

static long
GetDriveEncoder(void)
{
#ifdef  CONVEX
    return( vexMotorPositionGet( DRIVE_LEFT_MOTOR ));
#else
    return( nMotorEncoder[ DRIVE_LEFT_MOTOR ] );
#endif
}

static long
GetCurrentTime(void)
{
#ifdef  CONVEX
    return( chTimeNow() );
#else
    return( nSysTime );
#endif
}

static void
SetMotor( int index, int speed )
{
#ifdef  CONVEX
    vexMotorSet( index, speed );
#else
    motor[ index ] = speed;
#endif
}

static long
GetArmSensor(void)
{
#ifdef  CONVEX
    return( vexAdcGet( armPot ) );
#else
    return( SensorValue[ armPot ] );
#endif
}

/*-----------------------------------------------------------------------------*/
/*  Arcade drive with two motors                                               */
/*-----------------------------------------------------------------------------*/

void
DriveSystemDrive( int forward, int turn )
{
    long drive_l_motor;
    long drive_r_motor;

    // Set drive
    drive_l_motor = forward + turn;
    drive_r_motor = forward - turn;

    // normalize drive so max is 127 if any drive is over 127
    int max = abs(drive_l_motor);
    if (abs(drive_r_motor)  > max)
        max = abs(drive_r_motor);
    if (max>127) {
        drive_l_motor = 127 * drive_l_motor / max;
        drive_r_motor = 127 * drive_r_motor  / max;
    }

    // Send to motors
    // left drive
    SetMotor( DRIVE_LEFT_MOTOR, drive_l_motor );

    // right drive
    SetMotor( DRIVE_RIGHT_MOTOR, drive_r_motor );
}

/*-----------------------------------------------------------------------------*/
/*  Drive forwards until encoder count has elapsed or timeout reached          */
/*  optionally stop motors at end                                              */
/*-----------------------------------------------------------------------------*/

void
DriveForwardsEncoder( int distance, bool stopdrive )
{
    long    cur_position;
    long    target_position;
    long    currentTime;

    cur_position = GetDriveEncoder();

    target_position = cur_position + distance;

    // Start driving - bias to left
    if(distance > 0)
        DriveSystemDrive(  DRIVE_FORWARD_SPEED, 0 );
    else
        DriveSystemDrive( -DRIVE_FORWARD_SPEED, 0 );

    // wait for end - 5 seconds maximum
    for( currentTime = GetCurrentTime(); currentTime > (GetCurrentTime() - DEFAULT_DRIVE_TIMEOUT);)
        {
        cur_position = GetDriveEncoder();
        if( distance >= 0)
            {
            if( cur_position >= target_position )
                break;
            }
        else
            {
            if( cur_position <= target_position )
                break;
            }

        wait1Msec(10);
        }

    // Stop driving
    if( stopdrive )
        DriveSystemDrive( 0, 0 );
}

/*-----------------------------------------------------------------------------*/
/*  Arm control                                                                */
/*-----------------------------------------------------------------------------*/

// Global to hold requested (target) arm position
static int  armRequestedValue;

// Arm limits
#define ARM_UPPER_LIMIT     2000
#define ARM_LOWER_LIMIT      600

/*-----------------------------------------------------------------------------*/
/*  Set requested arm position and clip to limits                              */
/*-----------------------------------------------------------------------------*/

static void
SetArmPosition( int position )
{
    // crude limiting to upper and lower values
    if( position > ARM_UPPER_LIMIT )
        armRequestedValue = ARM_UPPER_LIMIT;
    else
    if( position < ARM_LOWER_LIMIT )
        armRequestedValue = ARM_LOWER_LIMIT;
    else
        armRequestedValue = position;
}

/*-----------------------------------------------------------------------------*/
/*  Get requested arm position                                                 */
/*-----------------------------------------------------------------------------*/

static int
GetArmPosition(void)
{
    return( armRequestedValue );
}

/*-----------------------------------------------------------------------------*/
/*  arm pid (actually just P) control task                                     */
/*-----------------------------------------------------------------------------*/

#ifdef CONVEX
task ArmPidController(void *arg)
#else
task ArmPidController()
#endif
{
           int    armSensorCurrentValue;
           int    armError;
           float  armDrive;
    static float  pid_K = 0.3;

#ifdef CONVEX
    (void)arg;
    vexTaskRegister("Arm task");
#endif

    while( true )
        {
        // Read the sensor value and scale
        armSensorCurrentValue = GetArmSensor();

        // calculate error
        armError =  armRequestedValue - armSensorCurrentValue;

        // calculate drive
        armDrive = (pid_K * (float)armError);

        // limit drive
        if( armDrive > 127 )
            armDrive = 127;
        else
        if( armDrive < (-127) )
            armDrive = (-127);

        // send to motor
        SetMotor( ARM_MOTOR, armDrive );

        // Don't hog cpu
        wait1Msec( 25 );
        }
}

/*-----------------------------------------------------------------------------*/
/*  Wait for arm to be in position                                             */
/*  returns 1 on success or 0 on timeout                                       */
/*-----------------------------------------------------------------------------*/

short
ArmSystemWaitInPosition( short timeout )
{
    int       armError;
    const int loopDelay = 50;

    // default timeout 3 seconds
    if(timeout <= 0)
        timeout = 3000;

    // convert mS to loops
    timeout /= loopDelay;

    while( timeout >= 0 )
        {
        // small delay
        wait1Msec( loopDelay );

        // calculate error
        armError =  GetArmPosition() - GetArmSensor();

        // Check the pid error for the arm
        if( (abs(armError) < 50 ) )
            return(1);

        // decrease timeout
        timeout--;
        }

    return(0);
}

void
AutonFun(void)
{
    // Start the Arm PID task
    StartTask( ArmPidController );


    // Arm going up
    SetArmPosition( 1000 );
    // start driving
    DriveForwardsEncoder( 1000, true );
    // wait for arm in position
    ArmSystemWaitInPosition( 3000 );

#ifdef CONVEX
    vex_printf("done");
#else
    writeDebugStreamLine("done");
#endif
}