/* File name: vehicle_rtc.ch
 * The purpose of this demo is to demonstrate the Ch Mindstorms
 * Control Package's ability to control the machine robot model,
 * As well as demonstrate how to get and use sensor data. */

#include <conio.h>
#include <stdio.h>
#include <mindstorms.h>

CMindstorms robot;
double speedRatio = 0.25;   //speedRatio of the motors. (default to 25)
int quit = 0;	              //used by quit case to exit the loop
char key = 'x',	            //stores the input from the user
     movemode = 'x';        //stores the last movement command

/* GUI display */
printf("Vehicle Direction:  Other Commands:");
printf("\n    [w]               [x] Stop Motors");
printf("\n [q] ^ [e]            [r] Exit Program");
printf("\n    \\|/               [1] Set SpeedRatio to 0.25");
printf("\n[a]<-|->[d]           [2] Set SpeedRatio to 0.50");
printf("\n     v                [3] Set SpeedRatio to 0.75");
printf("\n    [s]               [4] Set SpeedRatio to 1\n");
printf("Please Enter command:");

/* Control loop. Interprets user command and does action */
while(!quit){
    key = _getch();
    if(key == 'w'){         //up
        robot.setJointSpeedRatios(NaN, speedRatio, speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 'w';
    }
    else if(key == 's'){   //down
        robot.setJointSpeedRatios(NaN, -speedRatio, -speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 's';
    }
    else if(key == 'd'){   //right
        robot.setJointSpeedRatios(NaN, -speedRatio, speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 'd';
    }
    else if(key == 'a'){   //left
        robot.setJointSpeedRatios(NaN, speedRatio, -speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 'a';
    }
    else if(key == 'q'){   //forward-left
        robot.setJointSpeedRatios(NaN, speedRatio, 0.7*speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 'q';
    }
    else if(key == 'e'){   //forward-right
        robot.setJointSpeedRatios(NaN, 0.7*speedRatio, speedRatio);
        robot.moveJointForeverNB(JOINT2);
        robot.moveJointForeverNB(JOINT3);
        movemode = 'e';
    }
    else if(key == 'x'){   //stop and hold joints
        robot.holdJoints();
        movemode = 'x';
    }
    else if(key == 'r'){   //quit
        printf("\nExiting program.\n");
        quit = 1;
    }
    else if(key == '1'){   //speedRatio 0.25
        speedRatio = 0.25;
        ungetch(movemode);
    }
    else if(key == '2'){   //speedRatio 0.50
        speedRatio = 0.50;
        ungetch(movemode);
    }
    else if(key == '3'){   //speedRatio 0.75
        speedRatio = 0.75;
        ungetch(movemode);
    }
    else if(key == '4'){   //speedRatio 1
        speedRatio = 1;
        ungetch(movemode);
    }
    else{
        printf("\nInvalid Input!\n");
    }
}