/* File name: humanoid_rtc.ch
 * The purpose of this demo is to demonstrate the CH Mindstorms
 * Control Package's ability to control the Humanoid Robot Model,
 * as well as demonstrate how to get sensor data. */
#include <conio.h>
#include <stdio.h>
#include <mindstorms.h>
int printGUI();
int printSensor(CMindstorms &robot);
CMindstorms robot;
double direction=1, speed=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

/* Set sensor types */
robot.setSensorTouch(PORT1, "Touch");
robot.setSensorSound(PORT2, "dBA");
robot.setSensorLight(PORT3, "Ambient");
robot.setSensorUltrasonic(PORT4, "Centimeter");

/* print control menu */
printGUI();

/* Control loop. Interprets user command and does action*/
while (quit != 1){
    robot.setJointSpeeds(NaN, direction*speed, direction*speed);
    key = _getch();
    if(key == 'w'){         //forward
        direction = 1;
        robot.moveForeverNB();
        movemode = 'w';
    }
    else if(key =='s'){    //backward
        direction = -1;
        robot.moveForeverNB();
        movemode = 's';
    }
    else if(key == 'd'){   //right
        robot.moveJointForeverNB(JOINT1);
    }
    else if(key == 'a'){   //left
        robot.moveJointForeverNB(JOINT1);
    }
    else if(key == 'q'){   //print sensor
        printSensor(robot);
    }
    else if(key == 'x'){   //stop and hold joints
        robot.holdJoints();
        movemode = 'x';
    }
    else if(key == 'r'){   //quit
        printf("\nExiting program.\nPress any key to exit.");
        quit = 1;
    }
    else if(key == '1'){   //speedRatio .25
        speed = 25;
        ungetch(movemode);
    }
    else if(key == '2'){   //speedRatio .50
        speed = 50;
        ungetch(movemode);
    }
    else if(key == '3'){   //speedRatio .75
        speed = 75;
        ungetch(movemode);
    }
    else if(key == '4'){   //speedRatio 1
        speed = 100;
        ungetch(movemode);
    }
    else{
        printf("\nInvalid Input!\n");
    }
}

int printGUI(){
    /* GUI display */
    printf("Vehicle Direction:  Other Commands:");
    printf("\n     [w]           [x] Stop all Motors");
    printf("\n      ^            [r] Exit Program");
    printf("\n      |            [1] Set Speed to 25");
    printf("\n     stop          [2] Set Speed to 50");
    printf("\n     [s]           [3] Set Speed to 75");
    printf("\n[a]<-head->[d]     [q] Get Sensor Data\n");
    printf("Please Enter command:");

    return 0;
}

int printSensor(CMindstorms &robot) {
    int touchValue = 0;
    int soundValue = 0;
    int lightValue = 0;
    double ultraValue = 0;

    /* get values from robot */
    robot.getSensorTouch(PORT1, touchValue);
    robot.getSensorSound(PORT2, soundValue);
    robot.getSensorLight(PORT3, lightValue);
    robot.getSensorUltrasonic(PORT4, ultraValue);

    /* display the values */
    if (touchValue > 0) {
        printf("\nThe touch sensor has been pressed.\n");
    } else {
        printf("\nThe touch sensor has not been pressed.\n");
    }

    printf("The distance is %lf.\n", ultraValue);
    printf("The light level is %d.\n", lightValue);
    printf("The sound level is %ddBA\n\n\n", soundValue);

    /* print control menu */
    printGUI();

    return 0;
}