#!/usr/bin/python3
# -*- coding: utf-8 -*-

########################################
# HXbot main program                   #
# EoF 2016 EoF@itphx.ru                #
########################################

import HX
import REflex
import HXjoystick
import HXpower
import HXserver
import HXcam
import HXlcd
import HXdb
import time
import smbus
import sys
import threading
import RPi.GPIO as GPIO


# MAIN CLASS
class HXbotClass():

    def __init__(self):
        super(HXbotClass, self).__init__()

        self.i2c_bus = None
        self.hxpower = None
        self.reflex = None
        self.server = None

        self.joystick = None
        self.hxcam = None
        self.hxlcd = None
        self.hxdb = None
        self.light = None
        self.sound = None
        self.log = None

        self.status = HX.STATUS_INIT

        self.hxpower_lock = threading.Lock()
        self.reflex_lock = threading.Lock()
        self.hxlcd_lock = threading.Lock()
        self.hxdb_lock = threading.Lock()

    def get_ready(self):
        result = HX.OK_RSP

        # Init I2C bus
        try:
            self.i2c_bus = smbus.SMBus(HX.I2C_BUS_NUMBER)
        except IOError:
            self.stop()
            return result | HX.I2C_BUS_ERR

        # Init hxpower
        try:
            self.hxpower = HXpower.HXpowerClass(self.i2c_bus, self.hxpower_lock)
        except IOError:
            self.stop()
            return result | HX.HXPOWER_ERR

        # Init reflex
        try:
            self.reflex = REflex.REflexClass(self)
        except IOError:
            self.stop()
            return result | HX.REFLEX_ERR

        # Init server
        try:
            self.server = HXserver.HXserverClass(self)
        except OSError:
            self.stop()
            return result | HX.SERVER_ERR

        # Init joystick
        try:
            self.joystick = HXjoystick.HXjoystickClass()
        except OSError:
            #self.joystick.stop()
            #self.joystick = None
            result |= HX.JOYSTICK_WRN

        # Init hxcam
        try:
            self.hxcam = HXcam.HXcamClass()
        except OSError:
            self.hxcam.stop()
            self.hxcam = None
            result |= HX.HXCAMERA_WRN

        # Init hxlcd
        try:
            self.hxlcd = HXlcd.HXlcdClass(self.i2c_bus, self.hxlcd_lock)
        except OSError:
            self.hxlcd.stop()
            self.hxlcd = None
            result |= HX.HXLCD_WRN

        # Init hxdb
        try:
            self.hxdb = HXdb.HXdbClass(self)
        except Exception:
            #self.hxdb.stop()
            self.hxdb = None
            result |= HX.HXDB_WRN

        # Init GPIO
        GPIO.setmode(GPIO.BOARD)
        #GPIO.setup(25, GPIO.OUT)

        # Set status
        self.status = True

        return result

    def start(self):
        if self.status != HX.STATUS_READY:
            return HX.BLK_RSP

        # Start hxpower
        self.hxpower.start()

        # Start reflex
        self.reflex.start()

        # Start server
        self.server.start()

        # Start joystick
        if self.joystick is not None:
            self.joystick.start()

        # Start camera
        if self.hxcam is not None:
            self.hxcam.start()

        # Start lcd
        if self.hxlcd is not None:
            self.hxlcd.start()

        # Start db
        if self.hxdb is not None:
            self.hxdb.start()

        self.status = HX.STATUS_RUNNING

        return HX.OK_RSP

    def stop(self):
        # Disable MV
        self.hxpower.disable_mv()

        # Disable camera
        if self.hxcam:
            # Stop process
            if self.hxcam.is_running:
                self.hxcam.stop()
            # Clear property
            self.hxcam = None

        # Stop reflex
        if self.reflex:
            # Stop process
            if self.reflex.is_running:
                self.reflex.stop()
            # Clear property
            self.reflex = None

        # Stop joystick
        if self.joystick:
            # Stop process
            if self.joystick.is_running:
                self.joystick.stop()
            # Clear property
            self.joystick = None

        # Stop server
        if self.server:
            # Stop process
            if self.server.is_running:
                self.server.stop()
            # Clear property
            self.server = None

        # Stop hxpower
        if self.hxpower:
            # Stop process
            if self.hxpower.is_running:
                self.hxpower.stop()
            # Clear property
            self.hxpower = None

        # Stop hxlcd
        if self.hxlcd:
            # Stop process
            if self.hxlcd.is_running:
                self.hxlcd.stop()
            # Clear property
            self.hxlcd = None

        # Stop hxdb
        if self.hxdb:
            # Stop process
            if self.hxdb.is_running:
                self.hxdb.stop()
            # Clear property
            self.hxdb = None

        # Close I2C bus
        if self.i2c_bus:
            self.i2c_bus.close()
            # Clear property
            self.i2c_bus = None

        # Set status
        self.status = HX.STATUS_STOP

        GPIO.cleanup()

        return HX.OK_RSP

    def get_temp(self):
        try:
            F = open("/sys/class/thermal/thermal_zone0/temp", "r")
            T = float(F.read())
        except IOError:
            return -999
        finally:
            F.close()

        return T / 1000


# MAIN PROGRAM
def main():
    # INIT
    print("Hi! I'am HXbot. Let's fun!'")
    print("Type command or \"help\"")

    # Create HXbot instance
    hxbot = HXbotClass()

    # Run
    start(hxbot)

    # MAIN LOOP
    while True:
        # Set command line view
        if hxbot.status == HX.STATUS_INIT:
            command_view = "INIT>"
        elif hxbot.status == HX.STATUS_READY:
            command_view = "READY>"
        elif hxbot.status == HX.STATUS_RUNNING:
            command_view = "RUNNING>"
        elif hxbot.status == HX.STATUS_STOP:
            command_view = "STOP>"
        elif hxbot.status == HX.STATUS_EXIT:
            command_view = "EXIT>"

        command = input(command_view)

        # COMMANDS
        # Help
        if command.lower() == "help":
            print_help()

        # Exit / Quit
        elif command.lower() == "exit" or \
             command.lower() == "quit":
            stop(hxbot)
            hxbot.status = HX.STATUS_EXIT
            print("Exit...")
            sys.exit()

        # Status
        elif command.lower() == "status":
            print_status(hxbot)
        # REflex control
        elif command.lower() == "reflex":
            reflex_control(hxbot)
        # HXpower control
        elif command.lower() == "hxpower":
            hxpower_control(hxbot)
        # HXcam control
        elif command.lower() == "hxcam":
            hxcam_control(hxbot)

        # Get ready
        elif command.lower() == "ready":
            ready(hxbot)
        # Start
        elif command.lower() == "start":
            start(hxbot)
        # Stop
        elif command.lower() == "stop":
            stop(hxbot)
        # Restart
        elif command.lower() == "restart":
            restart(hxbot)

        # Test
        elif command.lower() == "t":
            t1 = time.clock()
            print("t1:", t1)
            time.sleep(5)
            t2 = time.clock()
            print("t2:", t2)
            dt = t2 - t1
            print("dt:", dt)

        # WAT?
        else:
            print("WRONG COMMAND")
            print("Try again or type \"help\"")

    # EXIT
    sys.exit()


# FUNCTIONS
def ready(hxbot):
    if hxbot.status != HX.STATUS_READY:
        print("Get ready...")
        result = hxbot.get_ready()
        print_result(result)
    else:
        print("HXbot already ready")


def start(hxbot):
    if hxbot.status != HX.STATUS_RUNNING:
        if hxbot.status != HX.STATUS_READY:
            ready(hxbot)

        print("Start...")
        result = hxbot.start()
        print_result(result)
    else:
        print("HXbot already running")


def stop(hxbot):
    if hxbot.status != HX.STATUS_STOP:
        print("Stop...")
        result = hxbot.stop()
        print_result(result)
    else:
        print("HXbot already stopped")


def restart(hxbot):
    stop(hxbot)
    ready(hxbot)
    start(hxbot)


def print_help():
    print("This is HXbot main program. You can type this commands:")
    print("help   - this help")
    print("exit |")
    print("quit    - stop HXbot and exit")
    print("status  - get status")
    print("start   - start HXbot")
    print("stop    - stop HXbot")
    print("restart - stop HXbot")
    print("reflex  - control REflex")
    print("hxpower - control HXpower")


def hxpower_help():
    print("This is HXpower control program. You can type this commands:")
    print("help  - this help")
    print("exit  - main menu")
    print("bt    - switch BT")
    print("ba    - switch BA")
    print("mv    - enable/disable moving")
    print("us    - enable/disable USB")
    print("0-255 - send raw command")


def hxcam_help():
    print("This is HXcam control program. You can type this commands:")
    print("help  - this help")
    print("exit  - main menu")
    print("start - start HXcam")
    print("stop  - stop HXcam")
    print("restart - restart HXcam")


def reflex_help():
    print("This is REflex control program. You can type this commands:")
    print("help  - this help")
    print("exit  - main menu")
    print("0-255 - send raw command")


def print_result(result):
    # Universal return codes
    if result == HX.OK_RSP:
        print("OK")
    elif result == HX.NO_RSP:
        print("WARNING: No response")
    elif result == HX.ERR_RSP:
        print("ERROR")
    elif result == HX.BLK_RSP:
        print("ERROR: Blocked")
    elif result == HX.CSE_RSP:
        print("ERROR: Wrong control sum")
    elif result == HX.BLK_RSP:
        print("ERROR: ")
    elif result == HX.IOE_RSP:
        print("ERROR: Input/output error")
    elif result == HX.TMO_RSP:
        print("ERROR: Timeout")

    # HXbot extended return codes
    elif result == HX.I2C_BUS_ERR:
        print("ERROR: I2C bus not ready")
    elif result == HX.HXPOWER_ERR:
        print("ERROR: HXpower not ready")
    elif result == HX.JOYSTICK_WRN:
        print("WARNING: Joystick not found")
    elif result == HX.REFLEX_ERR:
        print("ERROR: REflex not ready")
    elif result == HX.SERVER_ERR:
        print("ERROR: Server not ready")
    elif result == HX.HXCAMERA_WRN:
        print("WARNING: Camera not ready")
    elif result == HX.HXLCD_WRN:
        print("WARNING: LCD module not ready")
    elif result == HX.HXDB_WRN:
        print("WARNING: DB module not ready")

    # Unknown return code
    else:
        print("WARNING: Unknown return code received")


def print_status(hxbot):
    if (hxbot.status != HX.STATUS_READY and
        hxbot.status != HX.STATUS_RUNNING):
        print("HXbot is not in ready or running state!")
        return

    # Print voltages
    print("Vcc =", hxbot.hxpower.vcc)
    print("Vin =", hxbot.hxpower.vin)
    print("Vdc =", hxbot.hxpower.vdc)
    print("Vbt =", hxbot.hxpower.vbt)
    print("Vba =", hxbot.hxpower.vba)
    print("Vpf =", hxbot.hxpower.vpf)
    print("Vzu =", hxbot.hxpower.vzu)

    # Print currents
    print("Abt =", hxbot.hxpower.abt)
    print("Aba =", hxbot.hxpower.aba)
    print("Ain =", hxbot.hxpower.ain)
    print("Ahx =", hxbot.hxpower.ahx)

    # Print temperature
    print("CPU Temp:", hxbot.get_temp())
    print("REflex Temp:", hxbot.reflex.get_temp())
    #print("HXPower Temp:", )

    # Print status
    if hxbot.hxpower.vcc_ok:
        print("Vcc - OK")
    else:
        print("Vcc - FAIL")
    if hxbot.hxpower.vin_ok:
        print("Vin - OK")
    else:
        print("Vin - FAIL")
    if hxbot.hxpower.vdc_ok:
        print("Vdc - OK")
    else:
        print("Vdc - FAIL")
    if hxbot.hxpower.vbt_ok:
        print("Vbt - OK")
    else:
        print("Vbt - FAIL")
    if hxbot.hxpower.vba_ok:
        print("Vba - OK")
    else:
        print("Vba - FAIL")
    if hxbot.hxpower.vpf_ok:
        print("Vpf - OK")
    else:
        print("Vpf - FAIL")
    if hxbot.hxpower.vzu_ok:
        print("Vzu - OK")
    else:
        print("Vzu - FAIL")

    if hxbot.hxpower.abt_ok:
        print("Abt - OK")
    else:
        print("Abt - FAIL")
    if hxbot.hxpower.aba_ok:
        print("Aba - OK")
    else:
        print("Aba - FAIL")
    if hxbot.hxpower.ain_ok:
        print("Ain - OK")
    else:
        print("Ain - FAIL")
    if hxbot.hxpower.ahx_ok:
        print("Ahx - OK")
    else:
        print("Ahx - FAIL")

    if hxbot.hxpower.bt_enabled:
        print("BT enabled")
    else:
        print("BT disabled")
    if hxbot.hxpower.ba_enabled:
        print("BA enabled")
    else:
        print("BA disabled")
    if hxbot.hxpower.mv_enabled:
        print("mv enabled")
    else:
        print("PF disabled")
    if hxbot.hxpower.us_enabled:
        print("USB power enabled")
    else:
        print("USB power disabled")
    if hxbot.hxpower.bt_blocked:
        print("BT blocked")
    else:
        print("BT unblocked")
    if hxbot.hxpower.ba_blocked:
        print("BA blocked")
    else:
        print("BA unblocked")

    if hxbot.hxpower.in_plugged:
        print("IN plugged")
    else:
        print("IN unplugged")

    if hxbot.hxpower.bt_full:
        print("BT charge complete")
    if hxbot.hxpower.ba_full:
        print("BA charge complete")

    print()


def reflex_control(hxbot):
    if (hxbot.status != HX.STATUS_READY and
        hxbot.status != HX.STATUS_RUNNING):
        print("HXbot is not in ready or running state!")
        return

    rawCommand = 0x00
    rawResponse = 0x00

    print("Reflex control program. Type command or \"help\"")
    print("Type \"exit\" to previous menu")
    command_view = "REflex control>"

    while True:
        command = input(command_view)
        if command.lower() == "exit":
            break
        elif command.lower() == "help":
            reflex_help()
        else:
            # Try to send RAW command to REflex
            try:
                rawCommand = int(command)
            except:
                rawCommand = 0x00

            if not rawCommand:
                continue

            with hxbot.reflex_lock:
                hxbot.i2c_bus.write_byte(HX.REFLEX_ADDRESS, rawCommand)
                print("Sent: ", rawCommand)
                time.sleep(HX.REFLEX_RESPONSE_DELAY)
                rawResponse = hxbot.i2c_bus.read_byte(HX.REFLEX_ADDRESS)
                print("Received: ", rawResponse)


def hxpower_control(hxbot):
    if (hxbot.status != HX.STATUS_READY and
        hxbot.status != HX.STATUS_RUNNING):
        print("HXbot is not in ready or running state!")
        return

    rawCommand = 0x00
    rawResponse = 0x00

    print("HXpower control program. Type command or \"help\"")
    print("Type \"exit\" to previous menu")
    command_view = "HXpower control>"

    while True:
        command = input(command_view)
        if command.lower() == "exit":
            break
        elif command.lower() == "help":
            hxpower_help()
        elif command.lower() == "bt":
            if hxbot.hxpower.bt_enabled:
                print("Disable BT...")
                result = hxbot.hxpower.disable_bt()
            else:
                print("Enable BT...")
                result = hxbot.hxpower.enable_bt()
            print_result(result)
        elif command.lower() == "ba":
            if hxbot.hxpower.ba_enabled:
                print("Disable BA...")
                result = hxbot.hxpower.disable_ba()
            else:
                print("Enable BA...")
                result = hxbot.hxpower.enable_ba()
            print_result(result)
        elif command.lower() == "mv":
            if hxbot.hxpower.mv_enabled:
                print("Disable MV...")
                result = hxbot.hxpower.disable_mv()
            else:
                print("Enable MV...")
                result = hxbot.hxpower.enable_mv()
            print_result(result)
        elif command.lower() == "us":
            if hxbot.hxpower.us_enabled:
                print("Disable USB power...")
                result = hxbot.hxpower.disable_us()
            else:
                print("Enable USB power...")
                result = hxbot.hxpower.enable_us()
            print_result(result)
        elif command.lower() == "reset":
            GPIO.output(25, 0)
            time.sleep(5)
            GPIO.output(25, 1)
        else:
            # Try to send RAW command to HXpower
            try:
                rawCommand = int(command)
            except:
                rawCommand = 0x00
                print("Wrong command!")

            if not rawCommand:
                continue

            rawCommand = (rawCommand << 8) | (rawCommand ^ HX.XOR_SEQ)
            try:
                with hxbot.hxpower_lock:
                    hxbot.i2c_bus.write_word_data(HX.POWER_ADDRESS,
                        HX.EXT_COM, rawCommand)
                    print("Sent:", rawCommand, "(", rawCommand, ")")
                    time.sleep(HX.POWER_RESPONSE_DELAY)
                    rawResponse = hxbot.i2c_bus.read_byte(HX.POWER_ADDRESS)
                    print("Received:", rawResponse)
            except IOError as E:
                print(E)


def hxcam_control(hxbot):
    if (hxbot.status != HX.STATUS_READY and
        hxbot.status != HX.STATUS_RUNNING):
        print("HXbot is not in ready or running state!")
        return

    print("HXcam control program. Type command or \"help\"")
    print("Type \"exit\" to previous menu")
    command_view = "HXcam control>"

    while True:
        command = input(command_view)
        if command.lower() == "exit":
            break
        elif command.lower() == "help":
            hxcam_help()
        elif command.lower() == "start":
            print("Start HXcam")
            if hxbot.hxcam.is_running:
                print("Allready running")
            else:
                hxbot.hxcam.start()
        elif command.lower() == "stop":
            print("Stop HXcam")
            if not hxbot.hxcam.is_running:
                print("Not running")
            else:
                hxbot.hxcam.stop()
        elif command.lower() == "restart":
            print("Restart HXcam")
            if hxbot.hxcam.is_running:
                print("Stop...")
                hxbot.hxcam.stop()
            if not hxbot.hxcam.is_running:
                print("Start...")
                hxbot.hxcam.start()

# MAIN PROGRAM
if __name__ == "__main__":
    main()