-- Inputs --
t = inputs.get(1)				--Time
FyR = inputs.get(3)				--GRF of right leg in y-direction
FyL = inputs.get(2)


-- Variables -- 
v_normal = 1.2					--normal walking speed
perturb_mag = 0.75				--perturbation (change in velocity)
threshold = 100					--minimum force from HS required to detect stance
delay = .075					--perturbation length
t_end = 300						--trial length
average_test = 6				--average number of gait cycles between perturbations
wait_time = 2
--perturbation_type = 10		--when perturbation will occur

-- Definition of states --
--state == 0			-- speed up the belt -- initial treadmill acceleartion
--state == 1			-- waiting for the first stance phase to end 
--state == 2  			-- stance phase during normal walking
--state == 3  			-- swing phase during normal walking
--state == 4  			-- perturbation time
--state == 5			-- perturbation ending
--state == 6 			-- wait state to ensure time between perturbations
--state == 7			-- after wait state, wait for next swing phase
--state == 8         	-- trial over, belt deceleration

ini = ini or 0 

if ini == 0 then 		--initialize 
	state = 0			--no heel strike
	test = 0
	perturb = 0
	t_zero = t			--time initialized 
	stance_duration = 0
	ini = 1
end

--Belt acceleration--
if state == 0 then  
	if hasaction("Start") then				--trigger start of IMU recording
		outputs.set("trigger",5)
	end
	total_stance = 0						--total of all stance times in trial, before perturbation
	n_stance = 0							--number of stance phase in trial, before perturbation
	v_right = (t - t_zero)*.2     			--belt speed
	v_left = (t - t_zero)*.2
	if v_right >= v_normal and v_left >= v_normal then 
		state = 1
	end
end 

-- wait until first swing phase--
if state == 1 then
	v_right = v_normal
	v_left = v_normal
	if FyR < threshold then			
		state = 3 
	end
end

--Swing phase--
if state == 3 then
	v_right = v_normal
	v_left = v_normal
	if FyR > threshold  then		
		t_heelstrike = t - t_zero					-- store time of heelstrike for stance duration calculation	
		state = 2									-- enter stance phase
		if t_heelstrike > 15 then					-- after 15s of normal walking, perturbations will begin
			test = math.random(1,average_test)		-- 1 in 6 chance of perturbation occuring (1 = perturbation, 2-6 = none)
			if test == 1 then						-- perturbation will occur
				perturb = math.random(1,8)			-- what kind of perturbation (1 = @10% stance, 2 = @20% stance, ... 8 = @80%)
				state = 4								-- enter perturbation state
			end
		end
	end
end

--Stance Phase--
if state == 2 then
	v_right = v_normal
	v_left = v_normal
	if FyR < threshold then
		state = 3 													-- enter swing phase
		if t - t_heelstrike > 0.3 then								-- prevent gait cycles where subjects step over onto other belt from counting toward duration
			total_stance = total_stance + (t-t_heelstrike) 			-- total time spent in stance
			n_stance = n_stance + 1 								-- # of stance phases that have occured
			stance_duration = total_stance/n_stance					-- stance duration calculation
		end
	end
end

--Perturbation Initiation--
if state == 4 then 
		if t >= t_heelstrike + (stance_duration * 0.1*perturb) then 				--perturbation start time
			v_right = v_normal + perturb_mag 										-- increase belt speed
			v_left = v_normal
			t_perturb = t															-- record time of perturbation
			state = 5
		else
			state = 4																-- stay in state until perturbation occurs
		end
end

--Perturbation End--
if state == 5 and t >= t_perturb + delay then	-- perturbation end time
	v_right = v_normal							-- belt speed decelerates back to normal
	v_left = v_normal		
	state = 6									-- Enter wait state
end

--Wait State (ensure there are minimum # of seconds between perturbations)--
if state == 6 then
	test = 0
	v_right = v_normal							-- maintain normal belt speed
	v_left = v_normal		
	if v_right == v_normal and t >= t_perturb + wait_time then
		state = 7
	end
end

--Exit Wait State--
if state == 7 and FyR < threshold then
	state = 3
end

--Trial time ending--
if state == 3 and t >= t_end then	-- trial end time
	state = 8 
end

--Decelerate Belt--
if state == 8 then
	v_right = v_normal - (t - t_end) *(0.2)
	v_left = v_normal - (t - t_end) *(0.2)
end
	
print(v_right, state, test, perturb, stance_duration)

outputs.set(1,v_right)
outputs.set(2,v_left)
outputs.set(3,state)
outputs.set(4,stance_duration)
outputs.set(5,test)
outputs.set(6,perturb)