# -*- coding: utf-8-sig -*-

######################################################################################################################

#

# EXPLORING THE EFFECT OF STEP LENGTH ASYMMETRY ON THE REACTIVE CONTROL OF STABILITY

#

# Lucas De Macedo Pinheiro ~ August, 2016

# Chang Liu ~ Oct, 2016 Modified

# Oct, 24

# Nov.10, 2016, fix the PTB latency

# detect right HS at t, send out pulse t=100ms, treadmill start acc at t=200ms, treadmill reach max speed at t=600ms, then treadmill dcc

# Dec.3, save step length

# Jan.20, add 60steps for step length adaption, and calculate the timing before PTB

# **Need to find the mapping of timing to actual elapse time, HS to HS=1.08sec elapse time=0.25

# Jun.10,2018 Figure out the multithreading issue using viz.director()

# ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

# Python code for the perturbations trial. It starts the treadmill according

# to "speed_S" values. A number of "ptb_max" perturbations are applied with

# "speed_P" velocities after a random number of steps between "step_range",

# left or right belt is also randomly chosen with the "belt_vec" list.

# Participants receive visual feedback based on the output of the baseline

# code. The same COP method is used. The program closes and the treadmill

# stops after all perturbations are applied.

# Inputs: "speed_S", "accel_max", "step_range", "speed_P", "SLA", "belt_vec"

#

######################################################################################################################

#

#

#

###############################################################################

from __future__ import division

import viz

import vizshape # 2D or 3D indicators

import vizact

import socket

import struct

import json

import time

import random

import numpy

import heapq

from labjack import ljm

import time

import timeit

import sys

from datetime import datetime

from time import sleep

import vizmultiprocess #Vizard does not support multiprocess but it has its own version 'vizmultiprocess'

##########################################################################################################################################################

# Parameters to change before the trial

accel_max = 20000 # Perturbations acceleration [mm/s] // make sure to change max accel in Treadmill Panel Settings

ptb_total= 5

belt_vec = random.sample(['L','R']*ptb_total, ptb_total*2) # 10 left + 10 right perturbations shuffled, # not count the last perturbation

step_range = (20,30) # min & max num steps until next perturbation min <= x < max

speed_S = (1000,1000) # Self-selected speed Standard speeds [ vLeft, Right ] [mm/s]

dv_forward = 300

dv_backward= -200

v_forward = [speed_S[i]+dv_forward for i in range(2)]

v_backward = [speed_S[i]+dv_backward for i in range(2)]

print(v_forward)

speed_P = random.sample([v_forward,v_backward]*ptb_total,ptb_total*2)

print(speed_P)

#speed_P = (1520,1500) # Perturbation speed

elapse_time=0.25

start_L=[]

start_R=[]

SLA =0 # enter value of desired asymmetry. If positive, left leg takes longer step and right takes shorter step to maitain stride legnth constant

print belt_vec

##########################################################################################################################################################

# Vizard window set up

viz.setMultiSample(4)

viz.setOption('viz.glFinish',1)

viz.MainWindow.fov(100)

viz.go()

viz.addChild('ground_wood.osgb')

################################################################################################################################

# Establish connection with Treadmill Control Panel

HOST = '127.0.0.1' #name of the target computer that runs the treadmill controller

PORT = 4000

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

s.connect((HOST, PORT))

# QTM initialization

QUALISYS_IP = '192.168.252.1'

qualisysOn = False

# Establish connection with LabJack

handle = ljm.openS("T7","USB", "ANY")

#Switch = ljm.openS("T7", "USB", "ANY")

## check if LabJack is communicating with the computer

name = "SERIAL_NUMBER"

result = ljm.eReadName(handle, name)

print("\neReadName result: ")

print(" %s = %f" % (name, result))

ljm.eWriteName(handle,"DAC0",0)

ljm.eWriteName(handle,"DAC1",0)

################################################################################################################################

def updateViewHQ():

vizact.onkeydown(' ',viz.cam.reset)

# targetL = vizshape.addQuad(size=(SL_Left,0.15),axis=vizshape.AXIS_Y, cullFace=False, color=viz.GREEN, pos=(SL_Left/2,0.01,0.7))

# targetL_bottom = vizshape.addQuad(size=(0.01,0.35),axis=vizshape.AXIS_Y, cullFace=False, color=viz.RED, pos=(SL_Left - 3*sd_Left,0.01,0.7))

# targetL_top = vizshape.addQuad(size=(0.01,0.35),axis=vizshape.AXIS_Y, cullFace=False, color=viz.RED, pos=(SL_Left + 3*sd_Left,0.01,0.7))

#

# targetR = vizshape.addQuad(size=(SL_Right,0.15),axis=vizshape.AXIS_Y, cullFace=False, color=viz.BLUE, pos=( SL_Right/2 ,0.01,0.3))

# targetR_bottom = vizshape.addQuad(size=(0.01,0.35),axis=vizshape.AXIS_Y, cullFace=False, color=viz.RED, pos=(SL_Right - 3*sd_Right,0.01,0.3))

# targetR_top = vizshape.addQuad(size=(0.01,0.35),axis=vizshape.AXIS_Y, cullFace=False, color=viz.RED, pos=(SL_Right + 3*sd_Right,0.01,0.3))

def qualisysInit():

qualisys = viz.add('qualisys.dle', 0, QUALISYS_IP)

return qualisys

# function to generate data packet to send to Treadmill Control Panel

def serializepacket(speedL,speedR,accL,accR,theta):

fmtpack = struct.Struct('>B 18h 27B')#should be 64 bits in length to work properly

outpack = fmtpack.pack(0,speedR,speedL,0,0,accR,accL,0,0,theta,~speedR,~speedL,~0,~0,~accR,~accL,~0,~0,~theta,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0)

return(outpack)

def receivePacket(recvPack):

return(unpack)

def qtm_receive():

return FP

# updated here; communicate with LabJack to send an impulse

def perturbation():

#print "NORMAL"

def labjack_impulse():

print "send pulse"

################################################################################################################################

def check_steps():

Rstp_flag = 1

# Map veloctity to voltage

def velocityToVoltage(velocityL,velocityR):

ljm.eWriteName(handle,"DAC1",outputVoltageR) ### fix

# return (outputVoltageL, outputVoltageR)

def acquireVelocity():

velocityToVoltage(speedL,speedR)

# print "speedR=",speedR

# print "speedL=",speedL

def labjack_impulse_L():

ljm.eWriteName(handle, "DAC0", 2)

time.sleep(0.01)

ljm.eWriteName(handle, "DAC0", 1)

def labjack_impulse_R():

ljm.eWriteName(handle, "DAC1", 2)

time.sleep(0.01)

ljm.eWriteName(handle, "DAC1", 1)

######################################################################################################################

#

# EQUATIONS FOR COP FROM ANALOG DATA

#

# Xcop = -XOffset + (-h*Fx - My)/Fz

# Ycop = -YOffset + (-h*Fy + Mx)/Fz

# h, XOFfset and YOffset are found in QTM Plate Calibration Config

# ** COP in Plate Coordinates **

#

######################################################################################################################

def StepLength(COP_L, COP_R, width=0.05, length=0.05):

flagR = 1

def check_step_timing(start_L, start_R):

print elapse_time

# f1 = open('C:\Users\User\Documents\Chang\elapse_time.txt','w')

# json.dump(diffs_L,f1)

# f1.close()

#########################################################################################################################

temp = raw_input('Is QTM recording?<y/n>')

if temp == 'y':

qualisys = qualisysInit()

qualisysOn = True

if qualisysOn:

# visual display variables

alpha = 0 # alpha in SLA formula

SL_Left = 0

SL_Right = 0

flagL = 0 #swing phase flags

flagR = 0

COP_L = [0,0]

COP_R = [0,0]

successL_count = 0

successR_count = 0

# Initial condition

updateViewHQ()

time.sleep(7)

# time.sleep(10) # delay [sec]

# perturbations variables

stp = 10 # 80steps initial instead of random.randint(step_range[0], step_range[1])

Lstp_flag = 0 # identify swing phase

Rstp_flag = 0

stp_counter = 0

belt = belt_vec[0] # initialization

ptb_max = len(belt_vec) # max number of perturbations

ptb = 1 # current perturbation

out = serializepacket(speed_S[0],speed_S[1],200,200,0)

s.sendall(out)

vizact.ontimer2(0,viz.FOREVER,StepLength,COP_L,COP_R,width=0.05,length=0.05)

vizact.ontimer2(0,viz.FOREVER,check_steps)

vizact.ontimer2(0,viz.FOREVER,acquireVelocity)

######################################################################################################################

#

# ? IMPROVEMENTS ?

#

# Use bars/lines instead of the black dots for feedback

# Create a countdown screen before treadmill starts/stops

# Change the success to percentage

# Add a success condition to apply perturbations, if success<threshold no perturbation

# Better integrate "check_steps" and "StepLength" functions (steps counter, "check_steps" as interruption)

#

######################################################################################################################