/**
  @file remote-drive.c
  @author Yves Hwang.
  @date 11/12/2001
  @brief Remote-controlled driving, kicking, camera movement and display.
 */
/****************************************
*       IRTV REMOTE SOCCERBOT DEMO      *
*       -=|remote-drive.c |=-           *
*                                       *
*       Remote-controlled driving,      *
*       kicking, camera movement and    *       
*       display.                        *
*                                       *
*       written by Yves Hwang           *
*       hwang-y@ee.uwa.edu.au           *       
*       11/12/2001                      *
*   Adapted by Thomas Braunl, June 2005 *
*****************************************/

#include <eyebot.h>
#include <math.h>
#include "irtv.h"


/* Nokia remote control codes */
#define CODE_0      0x6142      /*shoot*/
#define CODE_1      0x61fd      /*curve forward left*/
#define CODE_2      0x6102      /*forward*/
#define CODE_3      0x60fd      /*curve forward right*/
#define CODE_4      0x6082      /*turn left*/
#define CODE_5      0x617d      /*stop*/
#define CODE_6      0x6182      /*turn right*/
#define CODE_7      0x607d      /*curve back left*/
#define CODE_8      0x6042      /*back*/
#define CODE_9      0x61bd      /*curve back right*/
    
#define CODE_END    0x61a2      /*end program*/

#define CODE_RED    0x61ad      
#define CODE_GREEN  0x616d
#define CODE_YELLOW 0x6032      
#define CODE_BLUE   0x602d      

#define CODE_PLAY   0x6112      
#define CODE_NEXT   0x61ed      /*Move eye-cam to the right*/
#define CODE_PREV   0x60ed      /*Move eye-cam to the left*/ 
#define CODE_STOP   0x606d      /*Clear screen*/
#define CODE_OK     0x6166      /*Toggle camera mode display*/

#define CODE_TIMER  0x619d      /*Iecrease forward_velocity*/
#define CODE_CLEAR  0x6012      /*Decrease forward_velocity*/
#define CODE_NEG    0x6122      /*Increase angular_velocity*/
#define CODE_POS    0x61dd      /*Decrease angular_velocity*/

#define DISTANCE 1.0            /*Eye-bot's travelling distance*/
#define TURN_ANGLE 2*M_PI       /*Eye-bot's turning angle in radians*/
#define CURVE_ANGLE M_PI/6      /*Eye-bot's curving angle in rads*/
#define DELTA_VELOCITY 0.1      /*Velocity increment*/
#define DELTA_OMEGA 0.1         /*Angular velocity increment*/
#define DELTA_CAM_MOVEMENT 1    /*Camera position increment*/

#define INIT_VEL 0.4            /*Initial Velocity*/
#define INIT_ANG_VEL 0.5        /*Initial Angular Velocity*/

#define MAX_VEL 1.0             /*Maximum velocity*/
#define MAX_ANG_VEL 1.0         /*Maximum angular veloctiy*/
#define MIN_VEL 0.1             /*Minimum velocity*/
#define MIN_ANG_VEL 0.1         /*Minimum angular velocity*/

#define INIT_CAM_POS 130        /*Initial camera position*/

/** Definition of movement for the servoes.
 *  The enums defines the maximum and minim angle for the servoes.   
 */
enum {
  SERVO_MIN_POSITION =   0, /**< enum value SERVO_MIN_POSITION. */
  SERVO_MAX_POSITION = 255  /**< enum value SERVO_MAX_POSITION. */
};


int main (void)
{
        VWHandle vw_handle;
        ServoHandle kicker_servo, camera_servo;
        colimage colimg;  /* picture to work on */
        image    greyimg; /* picture for LCD-output */

        int camera, code, cam_position, graphic_mode_toggle=TRUE; 
        float forward_velocity, angular_velocity;
        
        forward_velocity = INIT_VEL;
        angular_velocity = INIT_ANG_VEL;
        cam_position = INIT_CAM_POS;
        
        LCDPrintf("Ronaldo Bot\nYves Hwang 2001\n");
        
        /* Initialize VW interface */
        vw_handle = VWInit(VW_DRIVE, 1);
        if (vw_handle == 0) {
                LCDPrintf("VWInit() error\n");
                return 1;
        }
        VWStartControl(vw_handle, 7, 0.3, 7, 0.1); /*optimum drive control*/

        /* Initialize IR remote control */
        if (IRTVInitHDT(IRTV))
        { LCDPrintf("IRTVInit\nError!\n"); return 1;
        }  
        /* Initialize servos for the kicking mechanism*/
        if (!(kicker_servo = SERVOInit(SERVO12)))
        { LCDPrintf("kicker_servoSerInit\nError!\n"); OSWait(100);
        }
        else SERVOSet(kicker_servo, SERVO_MIN_POSITION);
        
        /* Initialize servos for the camera*/
        if(!(camera_servo = SERVOInit(SERVO11)))
        { LCDPrintf("CameraSerInit\nError!\n"); OSWait(100);
        }
        else SERVOSet(camera_servo, cam_position);
        
        /* Initialize camera for imaging*/
        camera=CAMInit(NORMAL);
        if (camera==NOCAM) LCDPrintf("No camera!\n");
        else if (camera==INITERROR) LCDPrintf("CAMInit!\n");
        CAMSet (FPS3_75, 0, 0);
        
        LCDPrintf("Ok, ready to go!\n");
        OSWait(100);
        LCDClear();
        LCDMenu("", "", "", "END");
        
        /* Main loop */
        do {    
                code = IRTVRead(); /* Get remote key from buffer */
                if (graphic_mode_toggle) /* in graphics mode only: */
                { LCDSetPos(2,10); LCDPutIntS(code,5);
                }
                switch (code) {
                        case CODE_OK:
                          LCDClear();
                          LCDMenu("", "", "", "END");
                          if(graphic_mode_toggle == FALSE) graphic_mode_toggle = TRUE;                          
                          else 
                          {
                                graphic_mode_toggle = FALSE;
                                LCDPrintf("Text Mode\n\n");
                                LCDPrintf("Current Speeds\n%0.2f m/s\n%0.2f rads/s\n\n",
                                forward_velocity, angular_velocity);
                          }             
                          break;
                          
                        case CODE_STOP:
                          LCDClear();
                          LCDMenu("", "", "", "END");
                          break;
                          
                        case CODE_NEXT:
                          do{
                                if(cam_position > SERVO_MIN_POSITION)
                                {
                                        cam_position = cam_position - DELTA_CAM_MOVEMENT;
                                        SERVOSet(camera_servo, cam_position);
                                        OSWait(1); /*Fine tunes the camera movement*/
                                }
                            }while (IRTVPressed() == CODE_NEXT);
                          break;
                          
                        case CODE_PREV:
                          do{
                                if(cam_position < SERVO_MAX_POSITION)
                                {
                                        cam_position = cam_position + DELTA_CAM_MOVEMENT;
                                        SERVOSet(camera_servo, cam_position);
                                        OSWait(1); /*Tunes the camera movement; more
                                        responsive*/
                                }
                            }while (IRTVPressed() == CODE_PREV);
                          break;
                        
                        case CODE_TIMER:
                          if(forward_velocity < MAX_VEL)
                          {     
                                forward_velocity=forward_velocity+DELTA_VELOCITY;
                                if(graphic_mode_toggle==FALSE)
                                        LCDPrintf("%0.2f m/s \n", forward_velocity);
                          }
                          else  if(graphic_mode_toggle==FALSE)
                                        LCDPrintf("Max speed at \n%0.2f m/s\n", forward_velocity);
                          break;
                        
                        case CODE_CLEAR:
                          if(forward_velocity > (MIN_VEL + DELTA_VELOCITY) )
                          {
                                forward_velocity=forward_velocity-DELTA_VELOCITY;
                                if(graphic_mode_toggle==FALSE)
                                        LCDPrintf("%0.2f m/s\n", forward_velocity);
                          }
                          else  if(graphic_mode_toggle==FALSE)
                                        LCDPrintf("Min speed at \n%0.2f m/s\n",forward_velocity);
                          break;
                          
                        case CODE_NEG:
                          if(angular_velocity < MAX_ANG_VEL)
                          {
                                angular_velocity=angular_velocity+DELTA_OMEGA;
                                if(graphic_mode_toggle==FALSE)
                                        LCDPrintf("%0.2f rad/s\n", angular_velocity);
                          }
                          else if(graphic_mode_toggle==FALSE)
                                LCDPrintf("Max speed at \n%0.2f rad/s\n",angular_velocity);
                          break;
                          
                        case CODE_POS:
                          if(angular_velocity > (MIN_ANG_VEL + DELTA_OMEGA) )
                          {
                                angular_velocity=angular_velocity - DELTA_OMEGA;
                                if(graphic_mode_toggle==FALSE) 
                                        LCDPrintf("%0.2f rad/s\n", angular_velocity);
                          }
                          else if(graphic_mode_toggle==FALSE)
                                LCDPrintf("Min speed at \n%0.2f rad/s\n",angular_velocity);
                          break;
                          
                        /* Go forward */
                        case CODE_2:
                          VWDriveStraight(vw_handle, DISTANCE, forward_velocity);
                          if(graphic_mode_toggle==FALSE) 
                                LCDPrintf(" Forward %0.2f ms\n", forward_velocity);
                          break;

                        /* Go backward */
                        case CODE_8:
                          VWDriveStraight(vw_handle, -DISTANCE, forward_velocity);
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Back %0.2f ms\n", forward_velocity);
                          break;

                        /* Turn left */
                        case CODE_4:
                          VWDriveTurn(vw_handle, TURN_ANGLE, angular_velocity);
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Left %0.2f rad/s\n", angular_velocity);
                          break;

                        /* Turn right */
                        case CODE_6:
                          VWDriveTurn(vw_handle, -TURN_ANGLE, angular_velocity);
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Right %0.2f rad/s\n", angular_velocity);
                          break;

                        /* Stop! */
                        case CODE_5:
                          VWSetSpeed(vw_handle, 0, 0);
                          if(graphic_mode_toggle==FALSE) LCDPrintf(" Stop\n");
                          break;
                
                        case CODE_0:
                          if(graphic_mode_toggle==FALSE) LCDPrintf(" Takes a shot!!\n");
                          SERVOSet(kicker_servo, SERVO_MAX_POSITION);
                          OSWait(20);
                          SERVOSet(kicker_servo, SERVO_MIN_POSITION);
                          
                          break;
                          
                        /*curve left*/
                        case CODE_1:
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Forward Left %0.2f m/s\n", forward_velocity); 
                          VWDriveCurve(vw_handle, DISTANCE, CURVE_ANGLE, forward_velocity);
                          break;
                          
                        case CODE_3:
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Forward Right %0.2f m/s\n", forward_velocity);
                          VWDriveCurve(vw_handle, DISTANCE, -CURVE_ANGLE, forward_velocity);
                          break;
                          
                        case CODE_7:
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Back Left %0.2f m/s\n", forward_velocity);
                          VWDriveCurve(vw_handle, -DISTANCE, CURVE_ANGLE, forward_velocity);
                          break;
                          
                        case CODE_9:
                          if(graphic_mode_toggle==FALSE)
                                LCDPrintf(" Back Right %0.2f m/s\n", forward_velocity);
                          VWDriveCurve(vw_handle, -DISTANCE, -CURVE_ANGLE, forward_velocity);
                          break;      
                }
                
                if(graphic_mode_toggle == TRUE)
                {
                        if (camera<COLCAM)
                        {       
                                CAMGetFrame(&greyimg); 
                                LCDPutGraphic(&greyimg);  
                        }
                        else
                        {       
                                CAMGetColFrame(&colimg,FALSE); 
                                LCDPutColorGraphic(&colimg);
                        }
                        LCDSetString(0,10,"Camera");
                        LCDSetString(1,10,"Mode");
                }
                
        /* Loop until one of the "end" keys is pressed */
        } while (code!=CODE_END && KEYRead()==0);

        /* Turn off motors and release resources */
        VWSetSpeed(vw_handle, 0.0, 0.0);
        VWStopControl(vw_handle);
        VWRelease(vw_handle);
        IRTVTerm();
        SERVORelease(kicker_servo);
        SERVORelease(camera_servo);
        CAMRelease();
        return 0;
}