#!/usr/bin/python3
#
#  Shake Charlie's Head
#  This routine runs at startup and shakes Charlie's head
#  to indicate Charlie's awake and that the servos work.

import os
import signal
import sys
import logging
from time import sleep
from gopigo3 import FirmwareVersionError
from easygopigo3 import EasyGoPiGo3

logging.basicConfig(level = logging.DEBUG)

# check if it's ran with Python3
assert sys.version_info[0:1] == (3,)

# Initially the vposition and hposition of the two servos
# is set to vcenter and hcenter respectively, then hposition and vposition
# are incremented/decremented to move the servos as commanded

vcenter = vposition = 100
hcenter = hposition = 97

# Set the movement step size

servo_step_size = 5

#  Start an instance of the GoPiGo robot class via
#  EasyGoPiGo3

try:
    gopigo3_robot = EasyGoPiGo3()
except IOError:
    logging.critical('\nGoPiGo3 is not detected.\n')
    sys.exit(1)
except FirmwareVersionError:
    logging.critical('\nGoPiGo3 firmware needs to be updated\n')
    sys.exit(2)
except Exception:
    logging.critical('\nUnexpected error when initializing GoPiGo3 object\n')
    sys.exit(3)

#  Add instantiate "servo" object
servo1 = gopigo3_robot.init_servo('SERVO1')
servo2 = gopigo3_robot.init_servo('SERVO2')

#  Generic "head movement" routine
def move_head(hposition, vposition):
    servo1.rotate_servo(hposition)
    sleep(0.25)
    servo2.rotate_servo(vposition)
    sleep(0.25)
    servo1.disable_servo()
    servo2.disable_servo()
    return(0)

# Center Charlie's head
def center_head():
    global vposition
    global vcenter
    global hposition
    global hcenter

    vposition = vcenter
    hposition = hcenter
    move_head(hposition, vposition)
    return(0)

# Shake Charlie's head - just to prove he's alive! ;)
def shake_head():
    global vposition
    global vcenter
    global hposition
    global hcenter

    vposition = vcenter
    hposition = hcenter

    logging.info("Shaking Charlie's Head From Side To Side\n")
    hposition = 120
    move_head(hposition, vposition)
    hposition = 74
    move_head(hposition, vposition)

    logging.info("Centering Charlie's head horizontally\n")
    center_head()

    logging.info("Moving Charlie's Head Up And Down\n")
    vposition = 120
    move_head(hposition, vposition)
    vposition = 66
    move_head(hposition, vposition)

    logging.info("Re-centering Charlie's head vertically\n")
    center_head()
    return(0)

#------------------------------------
# Main routine entry point is here
#------------------------------------

if __name__ == "__main__":
    #Shaking Charlie's head to indicate startup
    center_head()
    sleep(0.25)
    shake_head()
    sleep(0.25)
    logging.info("Ready to go!\n")
    
    # check to see which kernel is being run
    os_version = os.popen('uname -m').read()
    print("os_version is", os_version)
    if os_version == "aarch64\n":
        logging.info("Operating System Kernel Version is 64 Bit\n")
        logging.info("Eyes are Blue\n")
        gopigo3_robot.left_eye_color = (0, 0, 65)  # set eyes blue if 64 bit
        gopigo3_robot.right_eye_color = (0, 0, 65)
    else:
        logging.info("Operating System Kernel Version is 32 Bit\n")
        logging.info("Eyes are Green\n")
        gopigo3_robot.left_eye_color = (0, 65, 0)  # otherwise, set to green if 32 bit
        gopigo3_robot.right_eye_color = (0, 65, 0)

    gopigo3_robot.open_eyes()  # enable eyes.
    
    sys.exit(0)