# devastator3.py
# 
# robot control for devastator
# 
# created 2019
# by hennisoft <http://www.hennisoft.ie>
# modified 07 Nov 2019 by
# Henning Buchholz
# 
# This example code is in the public domain.
# Based on:
# https://github.com/miguelgrinberg/Flask-SocketIO

import subprocess
import time
import smbus
import RPi.GPIO as GPIO
from threading import Thread
from flask import Flask, render_template, session, request
from flask_socketio import SocketIO, emit
import UltraBorg
import ThunderBorg

bus = smbus.SMBus(1)
device = 0x60
CAMERA_LED = 40
LED = 7
IR_LED = 11

# Power settings
voltageIn = 13.6                    # Total battery voltage to the ThunderBorg
voltageOut = 6.0                        # 6v Motors
maxPower = voltageOut / float(voltageIn)# 0.5 for 6v Motor and 12V input
percentMotor = maxPower / 100           # percent factor
motorVolts = voltageOut / maxPower      # motor reading to Volts
centerx = 0.0  # servo1 startposition
centery = 0.3  # servo2 startposition
ip = str(subprocess.check_output(["hostname", "-I"]).split()[0]) # get IP address


GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

GPIO.setup(CAMERA_LED,GPIO.OUT)
GPIO.setup(LED,GPIO.OUT)
GPIO.setup(IR_LED,GPIO.OUT)

GPIO.output(CAMERA_LED,True) # active low
GPIO.output(LED,False)
GPIO.output(IR_LED,False)

TB = ThunderBorg.ThunderBorg()
TB.Init()
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

time.sleep(0.5)
#okay = UB.SetWithRetry(UB.SetServoStartup2, UB.GetServoStartup2, 3290, 5)
UB.SetServoPosition1(centerx)
UB.SetServoPosition2(centery)


app = Flask(__name__)
app.debug = True
app.config['SECRET_KEY'] = 'secret!'
socketio = SocketIO(app)
thread = None


def background_stuff():
    """ Let's do it a bit cleaner """
    while True:
        time.sleep(0.3)
        b1 = bus.read_byte_data(device, 0x02)
        b2 = bus.read_byte_data(device, 0x03)
        pit = bus.read_byte_data(device, 0x04)
        rol = bus.read_byte_data(device, 0x05)
        pit = pit-256 if pit > 127 else pit
        rol = rol-256 if rol > 127 else rol
        dir = float(int((b1 << 8) + b2))/10

         # not sure which one is more exact (in theory: active one, but commented one is faster)
        #usm1 = float(int(UB.GetRawDistance1()))/10
        #usm2 = float(int(UB.GetRawDistance2()))/10
        #usm3 = float(int(UB.GetRawDistance3()))/10
        #usm4 = float(int(UB.GetRawDistance4()))/10
        usm1 = float(int(UB.GetDistance1()))/10
        usm2 = float(int(UB.GetDistance2()))/10
        usm3 = float(int(UB.GetDistance3()))/10
        usm4 = float(int(UB.GetDistance4()))/10

        battCurrent = round(TB.GetBatteryReading(),2)
        leftmotor = round(TB.GetMotor1() * motorVolts,2)
        rightmotor = round(TB.GetMotor2() * motorVolts,2)
        socketio.emit('message', {'direction': dir, 'pitch': pit, 'roll': rol, 'distfront': usm1, 'distleft': usm2, 'distright': usm3, 'distrear': usm4, 'batt': battCurrent, 'lmotor': leftmotor, 'rmotor': rightmotor}, namespace='/test')


@app.route('/')
def index():
    global thread
    if thread is None:
        thread = Thread(target=background_stuff)
        thread.start()
    return render_template('devastator2.html')

@socketio.on('my event', namespace='/test')
def my_event(msg):
    print 'Message: ', msg['data']

@socketio.on('motor', namespace='/test')
def my_motor(msg):
    print 'motor-left:  ', msg['left']
    print 'motor-right: ', msg['right']
    TB.SetMotor1(int(msg['left']) * percentMotor)
    TB.SetMotor2(int(msg['right']) * percentMotor)

@socketio.on('servo', namespace='/test')
def my_servo(msg):
    print 'servo-lr:  ', msg['servoh']
    print 'servo-ud:  ', msg['servov']
    UB.SetServoPosition1(float(int(msg['servoh']) - 50) / -50.0)
    UB.SetServoPosition2(float((int(msg['servov']) - 50) / -50.0)+centery)

@socketio.on('centermode', namespace='/test')
def my_center(msg):
    if (msg['on']=='True'):
        UB.SetServoPosition1(centerx)
        UB.SetServoPosition2(centery)

@socketio.on('irmode', namespace='/test')
def my_ir(msg):
    if (msg['on']=='True'):
        GPIO.output(CAMERA_LED,False)
        GPIO.output(IR_LED,True)
    else:
        GPIO.output(CAMERA_LED,True)
        GPIO.output(IR_LED,False)

@socketio.on('ledmode', namespace='/test')
def my_led(msg):
    if (msg['on']=='True'):
        GPIO.output(LED,True)
    else:
        GPIO.output(LED,False)

@socketio.on('guidedmode', namespace='/test')
def my_guided(msg):
    if (msg['on']=='True'):
        TB.MotorsOff()
    else:
        TB.MotorsOff()
 

@socketio.on('disconnect', namespace='/test')
def test_disconnect():
    print('Client disconnected - Motors: off')
    TB.MotorsOff()


if __name__ == '__main__':
    try:
        print '\n\nReady ! ...\n\n'
        print "please use: http://{}:8000".format(ip)
        print('in your browser to navigate Devastator !\n\n')
        print('to quit: close browser session and <CTRL>-c>\n\n')
        socketio.run(app, host='0.0.0.0', port=8000)
    except KeyboardInterrupt:
        print '\n... stopped by Ctrl+C'
    finally:
        #clean up
        print '... shutting down all services'
        socketio.stop()
        TB.MotorsOff()
        UB.SetServoPosition1(0.0)
        UB.SetServoPosition2(0.0)
        GPIO.cleanup()
        print '... dome ! See you next time ...'