/////////////////////////////////////////////////////////////
// default RCodePlus Program
// version 7 (.01) for ERS-7 and RCODE-7 (Not RCodePlus)
// released with YART 7.01
// Features removed from RCodePlus (2x0/31x) version marked with '//REMOVED: '

// YART compatible
    // that's what the //{ //} things are for

// "Mini-Obey Cat 7" - VERY MINIMAL
// Mini Obey Cat provide a very basic personality
//  You can customize this using any text editor
//  Please read the comments in this file to figure out what is happening

// NOTES:
// - provides simple functionality using built-in motions and sounds
// - a LAN connection is helpful for debugging
// - works on ERS-7 model AIBOs

/////////////////////////////////////////////////////////////

:Main
    // This is where the program starts

    GLOBAL doPrint 0

    VLOAD doPrint
    IF AiboId != 0 THEN
        // LAN is connected, print status
        SET doPrint 1
    ENDIF

    //REMOVED: no debug mode

    IF doPrint != 0 THEN
        PRINT "RCODE Basic YART Program 7 [.01]"
        PRINT "Program will PRINT status information"
    ENDIF

    IF AiboType != 7 THEN
        PRINT "only works on ERS-7"
        HALT
    ENDIF

    PRINT "-"

    //REMOVED: no RCR_Init without RCodePlus - learning new names not supported
    //REMOVED: AP_Vocab, AP_VLang features - only 1 vocabulary for RCODE 7 (right now)
    //REMOVED: AP_DEVCTL for HFD - not needed

    //{ "Main/Startup(once)"
      PLAY ACTION LIE // lie down at start
      WAIT
    //}

    // clear globals and sensors

    CLR SENSORS
    LOCAL lastPink 0
    LOCAL lastBone 0
    LOCAL idleCount 0

    LOCAL postureOld -1
    LOCAL postureNow -1

    // Main Loop
    WHILE 1 = 1

        // Voice and sound
        // do not change the code here
        //  instead edit "????_Handler"

        IF AU_Voice > 0 THEN
            PUSH AU_Voice_ID
            //REMOVED: AP_Voice_Level, AP_Voice_Pitch, AP_Voice_Horz
            CALL Voice_Handler:1
            SET AU_Voice 0
            LET idleCount 0 
        ENDIF

        //REMOVED: no AP_Loud

        IF AU_AiboTone > 0 THEN
            PUSH AU_AiboTone_ID
            CALL AiboTone_Handler:1
            SET AU_AiboTone 0
            LET idleCount 0 
        ENDIF

        IF AU_AiboSound > 0 THEN
            PUSH AU_AiboSound_ID
            CALL AiboSound_Handler:1
            SET AU_AiboSound 0
            LET idleCount 0 
        ENDIF

        //REMOVED: no AP_Noise, AP_Rhythm

        IF Pink_Ball <> 0 THEN
            IF lastPink == 0 THEN
                PUSH Pink_Ball_H
                PUSH Pink_Ball_V
                PUSH Pink_Ball_D
                CALL PinkBall_Found_Handler:3
                SET lastPink 1
            ENDIF
            PUSH Pink_Ball_H
            PUSH Pink_Ball_V
            PUSH Pink_Ball_D
            CALL PinkBall_Any_Handler:3
            LET idleCount 0 
        ELSE
            IF lastPink != 0 THEN
                CALL PinkBall_Lost_Handler
                SET lastPink 0
                LET idleCount 0 
            ENDIF
        ENDIF

        // Bone just like ball
        IF AIBONE <> 0 THEN
            IF lastBone == 0 THEN
                PUSH AIBONE_H
                PUSH AIBONE_V
                PUSH AIBONE_D
                CALL Bone_Found_Handler:3
                SET lastBone 1
            ENDIF
            PUSH AIBONE_H
            PUSH AIBONE_V
            PUSH AIBONE_D
            CALL Bone_Any_Handler:3
            LET idleCount 0 
        ELSE
            IF lastBone != 0 THEN
                CALL Bone_Lost_Handler
                SET lastBone 0
                LET idleCount 0 
            ENDIF
        ENDIF

        // simple Face change only
        //REMOVED: AP_FaceDetect with nearness and direction
        //REMOVED: face lost logic
        IF Face <> 0 THEN
			CALL Face_Found_Handler
            SET Face 0
			LET idleCount 0 
        ENDIF

        //REMOVED: other colors (AP_COLORLVL)

        ////////////////////////////////
        // ERS-7 sensors

        IF Head_ON <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: Head_ON"
            ENDIF
            //{ "Sensor/Head pressed"
            //}
            SET Head_ON 0
            LET idleCount 0 
        ENDIF

        IF Jaw_ON <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: Jaw_ON"
            ENDIF
            //{ "Sensor/Jaw pressed"
              PLAY ACTION TURN 180
              WAIT
            //}
            SET Jaw_ON 0
            LET idleCount 0 
        ENDIF

        IF BackF_ON <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackF_ON"
            ENDIF
            //{ "Sensor/BackFront pressed"
              PLAY ACTION STAND
              WAIT
            //}
            SET BackF_ON 0
            LET idleCount 0 
        ENDIF

        IF BackM_ON <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackM_ON"
            ENDIF
            //{ "Sensor/BackMiddle pressed"
              PLAY ACTION SIT
              WAIT
            //}
            SET BackM_ON 0
            LET idleCount 0 
        ENDIF

        IF BackR_ON <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackR_ON"
            ENDIF
            //{ "Sensor/BackRear pressed"
              PLAY ACTION LIE
              WAIT
            //}
            SET BackR_ON 0
            LET idleCount 0 
        ENDIF

        IF Back_Pat <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: Back_Pat"
            ENDIF
            //{ "Sensor/Back Pat"
              PLAY ACTION STAND
              WAIT
            //}
            SET Back_Pat 0
            LET idleCount 0 
        ENDIF

        IF BackR_Hit <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackR_Hit"
            ENDIF
            //{ "Sensor/Back Rear Hit"
            //}
            SET BackR_Hit 0
            LET idleCount 0 
        ENDIF

        IF Head_LONG <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: Head_LONG"
            ENDIF
            //{ "Sensor/Head press long time"
              PLAY ACTION LIE
              WAIT
            //}
            SET Head_LONG 0
            LET idleCount 0 
        ENDIF

        IF BackFR_LONG <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackFR_LONG"
            ENDIF
            //{ "Sensor/BackFront+Rear long time"
              PLAY ACTION LIE
              WAIT
            //}
            SET BackFR_LONG 0
            LET idleCount 0 
        ENDIF

        IF BackF_Jaw_LONG <> 0 THEN
            IF doPrint <> 0 THEN
                PRINT "Sensor: BackF_Jaw_LONG"
            ENDIF
            //{ "Sensor/BackFront+Jaw long time"
              PLAY ACTION LIE
              WAIT
            //}
            SET BackF_Jaw_LONG 0
            LET idleCount 0 
        ENDIF

        LET postureOld postureNow
        LET postureNow Posture1  // posture in case it has changed

        IF postureNow == 3 THEN
            // currently in the LIE DOWN position
            // front paws can be used for input
            IF postureOld <> 3 THEN
                IF doPrint <> 0 THEN
                    PRINT "postureNew=SLEEP - will respond to front paws"
                ENDIF
                SET RFLeg_ON 0 // clear old values
                SET LFLeg_ON 0
            ENDIF

            IF RFLeg_ON <> 0 THEN
                IF doPrint <> 0 THEN
                    PRINT "Sensor: Front RFLeg_ON (LIEDOWN)"
                ENDIF
                //{ "Sensor/RF Paw (sleep position)"
                //}
                SET RFLeg_ON 0
                LET idleCount 0 
            ENDIF

            IF LFLeg_ON <> 0 THEN
                IF doPrint <> 0 THEN
                    PRINT "Sensor: Front LFLeg_ON (LIEDOWN)"
                ENDIF
                //{ "Sensor/LF Paw (sleep position)"
                //}
                SET LFLeg_ON 0
                LET idleCount 0 
            ENDIF
        ENDIF


//TOADD: new Distance and Distance_Cliff logic
//TOADD: ? maybe something with brightness ?

        ADD idleCount 1
        IF idleCount > 100 THEN
            // 100 ticks (approx 10 seconds) with nothing happening
            IF doPrint <> 0 THEN
                PRINT "Idle: postureNow = %d" postureNow
            ENDIF
            IF postureNow == 1 THEN // STAND
                CALL DoSomethingRandom_Stand
            ENDIF
            IF postureNow == 2 THEN // SIT
                CALL DoSomethingRandom_Sit
            ENDIF
            IF postureNow == 3 THEN // LIE / SLEEP
                CALL DoSomethingRandom_Sleep
            ENDIF
            // otherwise walking or something else
            LET idleCount 0
        ENDIF

        WAIT 100
    WEND // end of main loop

EXIT

/////////////////////////////////////////////////////////////
// Random skits after 10 seconds of idleness

:DoSomethingRandom_Stand
    LOCAL randVal
    RND randVal 1 5
    IF randVal == 1 THEN
        //{ "Idle/Stand Random#1"
          PLAY ACTION WALK 0 500
          WAIT
        //}
    ENDIF
    IF randVal == 2 THEN
        //{ "Idle/Stand Random#2"
        //}
    ENDIF
    IF randVal == 3 THEN
        //{ "Idle/Stand Random#3"
        //}
    ENDIF
    IF randVal == 4 THEN
        //{ "Idle/Stand Random#4"
          PLAY ACTION SIT // I'm tired of standing around
          WAIT
        //}
    ENDIF
    IF randVal == 5 THEN
        //{ "Idle/Stand Random#5"
          PLAY ACTION LIE // I'm tired of standing around
          WAIT
        //}
    ENDIF
RETURN

:DoSomethingRandom_Sit
    LOCAL randVal
    RND randVal 1 5
    IF randVal == 1 THEN
        //{ "Idle/Sit Random#1"
        //}
    ENDIF
    IF randVal == 2 THEN
        //{ "Idle/Sit Random#2"
        //}
    ENDIF
    IF randVal == 3 THEN
        //{ "Idle/Sit Random#3"
        //}
    ENDIF
    IF randVal == 4 THEN
        //{ "Idle/Sit Random#4"
          PLAY ACTION STAND // Let's get moving
          WAIT
        //}
    ENDIF
    IF randVal == 5 THEN
        //{ "Idle/Sit Random#5"
          PLAY ACTION LIE // I'm tired of sitting around
          WAIT
        //}
    ENDIF
RETURN

:DoSomethingRandom_Sleep // AKA "lie down"
    LOCAL randVal
    RND randVal 1 5
    IF randVal == 1 THEN
        //{ "Idle/Sleep Random#1"
        //}
    ENDIF
    IF randVal == 2 THEN
        //{ "Idle/Sleep Random#2"
        //}
    ENDIF
    IF randVal == 3 THEN
        //{ "Idle/Sleep Random#3"
        //}
    ENDIF
    IF randVal == 4 THEN
        //{ "Idle/Sleep Random#4"
          PLAY ACTION STAND
          WAIT
        //}
    ENDIF
    IF randVal == 5 THEN
        //{ "Idle/Sleep Random#5"
          PLAY ACTION SIT // Get up
          WAIT
        //}
    ENDIF
RETURN


/////////////////////////////////////////////////////////////
// _Handler routines - YART compatible

:Voice_Handler
  ARG id

  IF doPrint <> 0 THEN
    PRINT "Voice_Handler: %d" id
  ENDIF

  IF id == 1 THEN
    //{ "AIBO voice command/'AIBO' or <dog's name>"
      PLAY ACTION KICK 30 1000
      WAIT
    //}
  ENDIF
  IF id == 0x100 THEN
    //{ "AIBO voice command/<owner's name>"
      PLAY ACTION KICK -30 1000
      WAIT
    //}
  ENDIF
  IF id == 2 THEN
    //{ "AIBO voice command/'What's your name?'"
    //}
  ENDIF
  IF id == 3 THEN
    //{ "AIBO voice command/'Say hello'"
    //}
  ENDIF
  IF id == 4 THEN
    //{ "AIBO voice command/'Shake paw'"
      PLAY ACTION KICK -30 1000
      WAIT
    //}
  ENDIF
  IF id == 5 THEN
    //{ "AIBO voice command/'Morning'"
    //}
  ENDIF
  IF id == 6 THEN
    //{ "AIBO voice command/'Hello'"
    //}
  ENDIF
  IF id == 7 THEN
    //{ "AIBO voice command/'Good night'"
    //}
  ENDIF
  IF id == 8 THEN
    //{ "AIBO voice command/'See you'"
    //}
  ENDIF
  IF id == 9 THEN
    //{ "AIBO voice command/'How are you?'"
    //}
  ENDIF
  IF id == 10 THEN
    //{ "AIBO voice command/'Hey AIBO'"
    //}
  ENDIF
  IF id == 11 THEN
    //{ "AIBO voice command/'Thanks'"
    //}
  ENDIF
  IF id == 12 THEN
    //{ "AIBO voice command/'Sorry'"
    //}
  ENDIF
  IF id == 13 THEN
    //{ "AIBO voice command/'Cheer up'"
    //}
  ENDIF
  IF id == 14 THEN
    //{ "AIBO voice command/'Banzai'"
    //}
  ENDIF
  IF id == 15 THEN
    //{ "AIBO voice command/'That's right'"
    //}
  ENDIF
  IF id == 16 THEN
    //{ "AIBO voice command/'That's wrong'"
    //}
  ENDIF
  IF id == 17 THEN
    //{ "AIBO voice command/'Good AIBO'"
    //}
  ENDIF
  IF id == 18 THEN
    //{ "AIBO voice command/'Don't do that'"
    //}
  ENDIF
  IF id == 19 THEN
    //{ "AIBO voice command/'Let's play!'"
    //}
  ENDIF
  IF id == 20 THEN
    //{ "AIBO voice command/'Sing a song'"
    //}
  ENDIF
  IF id == 21 THEN
    //{ "AIBO voice command/'Dance'"
    //}
  ENDIF
  IF id == 22 THEN
    //{ "AIBO voice command/'Show time'"
    //}
  ENDIF
  IF id == 23 THEN
    //{ "AIBO voice command/'Pose for me'"
    //}
  ENDIF
  IF id == 24 THEN
    //{ "AIBO voice command/'Clown around'"
    //}
  ENDIF
  IF id == 25 THEN
    //{ "AIBO voice command/'Show off'"
    //}
  ENDIF
  IF id == 26 THEN
    //{ "AIBO voice command/'Say message'"
    //}
  ENDIF
  IF id == 27 THEN
    //{ "AIBO voice command/'Let's be secret'"
    //}
  ENDIF
  IF id == 28 THEN
    //{ "AIBO voice command/'Open sesame'"
    //}
  ENDIF
  IF id == 29 THEN
    //{ "AIBO voice command/'Happy day'"
    //}
  ENDIF
  IF id == 30 THEN
    //{ "AIBO voice command/'Stand up'"
      PLAY ACTION STAND
      WAIT
    //}
  ENDIF
  IF id == 31 THEN
    //{ "AIBO voice command/'Lie down'"
      PLAY ACTION LIE
      WAIT
    //}
  ENDIF
  IF id == 32 THEN
    //{ "AIBO voice command/'Sit down'"
      PLAY ACTION SIT
      WAIT
    //}
  ENDIF
  IF id == 33 THEN
    //{ "AIBO voice command/'Turn right'"
      PLAY ACTION TURN -90
      WAIT
    //}
  ENDIF
  IF id == 34 THEN
    //{ "AIBO voice command/'Turn left'"
      PLAY ACTION TURN 90
      WAIT
    //}
  ENDIF
  IF id == 35 THEN
    //{ "AIBO voice command/'Go forward'"
      PLAY ACTION WALK 0 500
      WAIT
    //}
  ENDIF
  IF id == 36 THEN
    //{ "AIBO voice command/'Go backward'"
      PLAY ACTION TURN 180
      WAIT
    //}
  ENDIF
  IF id == 37 THEN
    //{ "AIBO voice command/'Go ahead'"
      PLAY ACTION WALK 0 100
      WAIT
    //}
  ENDIF
  IF id == 38 THEN
    //{ "AIBO voice command/'Stop'"
    //}
  ENDIF
  IF id == 39 THEN
    //{ "AIBO voice command/'Faster'"
    //}
  ENDIF
  IF id == 40 THEN
    //{ "AIBO voice command/'Slow down'"
    //}
  ENDIF
  IF id == 41 THEN
    //{ "AIBO voice command/'Pink ball'"
    //}
  ENDIF
  IF id == 42 THEN
    //{ "AIBO voice command/'Right leg kick'"
      PLAY ACTION KICK -30 1000
      WAIT
    //}
  ENDIF
  IF id == 43 THEN
    //{ "AIBO voice command/'Right leg touch'"
    //}
  ENDIF
  IF id == 44 THEN
    //{ "AIBO voice command/'Left leg kick'"
      PLAY ACTION KICK 30 1000
      WAIT
    //}
  ENDIF
  IF id == 45 THEN
    //{ "AIBO voice command/'Left leg touch'"
    //}
  ENDIF
  IF id == 46 THEN
    //{ "AIBO voice command/'Ready set go'"
    //}
  ENDIF
  IF id == 47 THEN
    //{ "AIBO voice command/'You won'"
    //}
  ENDIF
  IF id == 48 THEN
    //{ "AIBO voice command/'You lost'"
    //}
  ENDIF
  IF id == 49 THEN
    //{ "AIBO voice command/'Action one'"
    //}
  ENDIF
  IF id == 50 THEN
    //{ "AIBO voice command/'Action two'"
    //}
  ENDIF
  IF id == 51 THEN
    //{ "AIBO voice command/'Action three'"
    //}
  ENDIF
  IF id == 52 THEN
    //{ "AIBO voice command/'Action four'"
    //}
  ENDIF
  IF id == 53 THEN
    //{ "AIBO voice command/'Action five'"
    //}
  ENDIF
//REVIEW: extended vocabulary

RETURN

///////////////////////
//REMOVED: Loud_Handler

///////////////////////

:AiboTone_Handler
  ARG id

  IF doPrint <> 0 THEN
    PRINT "AiboTone_Handler: %d" id
  ENDIF

RETURN

///////////////////////

:AiboSound_Handler
  ARG id

  IF doPrint <> 0 THEN
    PRINT "AiboSound_Handler: %d" id
  ENDIF

  //{ "AIBO hears/another AIBO"
  //}
RETURN

///////////////////////

//REMOVED: Noise_Handler, Rhythm_Handler

///////////////////////
// Pink Ball

:PinkBall_Found_Handler
  ARG hangle
  ARG vangle
  ARG dist

  IF doPrint <> 0 THEN
    PRINT "PinkBall_Found_Handler (%d %d) %d" hangle vangle dist
  ENDIF

  //{ "AIBO sees/Pink Ball (seen)"
    PLAY ACTION STAND
    WAIT
    PLAY ACTION WALK 0 100
    WAIT
  //}
RETURN

:PinkBall_Lost_Handler
  IF doPrint <> 0 THEN
    PRINT "PinkBall_Lost_Handler (%d %d) %d" hangle vangle dist
  ENDIF
  //{ "AIBO sees/Pink Ball (lost)"
    PLAY ACTION SIT
    WAIT
  //}

RETURN

:PinkBall_Any_Handler // called quite often when tracking
  ARG hangle
  ARG vangle
  ARG dist

  //{ "AIBO sees/Pink Ball (tracking)"
  //}

RETURN

///////////////////////
// Bone

:Bone_Found_Handler
  ARG hangle
  ARG vangle
  ARG dist

  IF doPrint <> 0 THEN
    PRINT "Bone_Found_Handler (%d %d) %d" hangle vangle dist
  ENDIF

  //{ "AIBO sees/Bone (seen)"
    PLAY ACTION STAND
    WAIT
    PLAY ACTION WALK 0 -100
    WAIT
  //}
RETURN

:Bone_Lost_Handler
  IF doPrint <> 0 THEN
    PRINT "Bone_Lost_Handler (%d %d) %d" hangle vangle dist
  ENDIF
  //{ "AIBO sees/Bone (lost)"
    PLAY ACTION SIT
    WAIT
  //}

RETURN

:Bone_Any_Handler // called quite often when tracking
  ARG hangle
  ARG vangle
  ARG dist

  //{ "AIBO sees/Bone (tracking)"
  //}

RETURN

///////////////////////
// Face

:Face_Found_Handler
  IF doPrint <> 0 THEN
	PRINT "Sensor: Saw your face"
  ENDIF
  //{ "AIBO sees/Human Face (seen)"
  //}
RETURN

/////////////////////////////////////////////////////////////
//REMOVED: Fav/UnFav handlers
//REMOVED: 'RCR' RCode Runtime, #AP_INCLUDE

/////////////////////////////////////////////////////////////
