%Autor. Grupo XITA Bros.
%Fecha. 14/05/2015
%
%Coordenadas x,y de los puntos A,B,C,D,E,F,G,H en los pasillos:
%            A----------------------------3----B
%                                      2
%                                   o  alfa
%            C------------D      o E-----------F
%                        /    o   /  theta
%                       /  o     /
%                      /o       /
%                     1        /
%                    /        /
%                   G        H
%
%
%El origen del plano cartesiano se considera en el punto E:
%             y
%             ^
%             |
%             |
%             E------>x
%
%La escalera está representada por la secuencia de círculos: o o o o o.
%Los puntos 1 y 2 son los extremos de la escalera.
%
%Se ha propuesto dividir el movimiento en dos fases:
%   a) Fase 1. Cuando la escalera se mueve de abajo hacia arriba, con
%   un extremo recargado en D-G y el otro orientado hacia el origen E.
%   b) Fase 2. Cuando la escalera ha terminado la Fase 1. Comienza
%   recargando su extremo 2 en E, y su extremo 1, aún en contacto con D-G. 
%   El movimiento continúa hasta que la escalera está complementamente
%   en posición horizontal, o ha quedado atorada entre los pasillos,
%   es decir con el extremo 1 en D-G y el extremo 2 en A-B. Siempre
%   apoyando un punto de ella en el origen E.
%
%El ángulo alfa se forma cuando la escalera tiene su extremo 1 recargado
%en el segmento D-G y otro punto recargado en el origen E.
%
%alfa:  Ángulo formado por B-E-F
%theta: Ángulo formado por F-E-H
%
% (x_1,y_1) y (x_2,y_2) son las coordenadas de los puntos 1 y 2 respectivamente.

clc
clear all
close all

%**************ENTRADAS DEL USUARIO******************************
a       = 5;            	%Ancho del pasillo inclinado.
b       = 7;            	%Ancho del pasillo horizontal.
theta_g = 120;				%Ángulo por los pasillos.En grados.Debe estar en el rango abierto (90,180).
%Intervalo de prueba. El usuario prueba con diferentes valores para 
%L en un rango dado. El incremento de L en cada intento es de delta_L.
L_min 	= 23;
L_max 	= 25;
delta_L = 0.1;

ANIMATION = true; 			%Poner el valor de false para que desactivar la animación y se ejecute más rápido el script.
							%Se recomienda asignar el valor de false para ángulos theta cercanos a 90 grados o a 180 grados.



%**************PARAMETROS DE LA ANIMACION*********************
delta_x = 0.1 ; %Cuánto cambia el extremo 1 de posición, sobre el eje x, en cada iteración.

FASE_1  = true; %Si FASE 1 es verdadera estará la escalera en la FASE 1, de lo contrario
                %estará en la FASE 2.
TERMINA_SIMULACION = false;

theta   = theta_g*pi/180;   %Ángulo formado por los pasillos.En radianes.

%*********COORDENADAS DE LOS PUNTOS A,B,C,D,E,F,G,H*******

Ax = -2*L_max;
Ay = b;

Bx = 1.5*L_max;
By = b;

Cx = Ax;
Cy = 0;

beta = theta - pi/2;
Dx = -a/cos(beta);
Dy = 0;

Ex = 0;
Ey = 0;

Fx = Bx;
Fy = 0;

Gx = -L_max;
Gy = - (a + Gx*sin(theta) )/cos(theta);

Hx = Gx + a/cos(beta);
Hy = Gy;


coord_x = [Ax,Bx,Cx,Dx,Ex,Fx,Ex,Fx,Gx,Hx];
coord_y = [Ay,By,Cy,Dy,Ey,Fy,Ey,Fy,Gy,Hy];
xmin = min(coord_x);
xmax = max(coord_x);
ymin = min(coord_y);
ymax = max(coord_y);
limits_xy = [ xmin-1 , xmax+1 , ymin-1 , ymax+1 ];


%*************** SEGMENTOS DE LOS PASILLOS***************
 ABx = [Ax,Bx];
 ABy = [Ay,By];
 
 CDx = [Cx,Dx];
 CDy = [Cy,Dy];
 
 GDx = [Gx,Dx];
 GDy = [Gy,Dy];
 
 HEx = [Hx,Ex];
 HEy = [Hy,Ey];
 
 EFx = [Ex,Fx];
 EFy = [Ey,Fy];
 
%******ECUACIONES DE LAS RECTAS QUE DEFINEN LOS PASILLOS*******

r2 = @(x,theta) -( a + x*sin(theta) )/cos(theta);   %Segmento G-D. Cuando el pasillo está inclinado.
r3 = @(x,theta) -x*tan(theta);                      %Fase 1. Segmento H-E .Cuando el pasillo está inclinado. 
r4 = @(x_2,x_1,y_1)  y_1 + (y_1*(x_2 - x_1))/x_1 ;  %Fase 2. 


for L = L_min : delta_L : L_max

	x_1 = Gx;
		
	while(1)
		if ANIMATION
			clf
			hold on
		end
			%**********Pasillos*************
			
			if ANIMATION
				plot(ABx,ABy);
				plot(CDx,CDy);
				plot(GDx,GDy);
				plot(HEx,HEy);
				plot(EFx,EFy);
			end
			
			%**********Escalera*************
			%Coordenada y del punto 1. La coordenada x, es x_1.
			y_1 = r2(x_1,theta);
			
			%Coordenada x y y del punto 2.
			if FASE_1
				x_2 = (x_1 + ...
					 (L^2*tan(theta)^2 + L^2 - x_1^2*tan(theta)^2 - 2*x_1*y_1*tan(theta) - y_1^2)^(1/2) -...
					  y_1*tan(theta))/(tan(theta)^2 + 1);
				y_2 = r3(x_2,theta);
			   
				if y_2>0 %Si se cumple esta condición, tiene que comenzar la FASE 2.
					FASE_1 = false;
				end
			   
			else %FASE 2
				x_2 = (x_1*(x_1^2 - L*(x_1^2 + y_1^2)^(1/2) + y_1^2))/(x_1^2 + y_1^2);
				y_2 = r4(x_2,x_1,y_1) ;
				
				cond_y1 = y_1>0 ;
				cond_y2 = y_2>b ;
				if cond_y1 || cond_y2
				
					if ANIMATION
						plot([x_1_old,x_2_old],[y_1_old,y_2_old],'r');
						axis(limits_xy);
						axis equal
						grid on
						title(['Intento con L = ',num2str(L,4)]);
					end
					
					if cond_y1
						fprintf('Eureka! Pasé con este valor de L = %.4f\n',L);
					end
					
					if cond_y2
						fprintf('No logré pasar con el valor de L = %.4f\n',L);
						TERMINA_SIMULACION = true;
					end
					
					break;
				end
			end
			if ANIMATION
				plot([x_1,x_2],[y_1,y_2],'r');
			end
		if ANIMATION
			hold off
			axis(limits_xy);
			axis equal
			grid on
			title(['Intento con L = ',num2str(L,4)]);
			pause(0.001);
		end
		
		x_1_old = x_1;
		y_1_old = y_1;
		x_2_old = x_2;
		y_2_old = y_2;
	   
		x_1 = x_1 + delta_x;
		

	end

	if TERMINA_SIMULACION == true
		break; %break del for
	end
	
end