%% preparo el espacio de trabajo
clear all
close all

%% Variables para configurar el programa
Comparar=true;
Compararforma=false;
Velocidades=false;

%% parametros y condiciones iniciales
g = 9.8;       %aceleracion de la gravedad (en m/s^2)
l=1;           %longitud del pendulo en m
titaini=pi/2;  %angulo incial
velini=0;      %velocidad angular inicial
t=10;         %tiempo final
m=1;           %masa del cuerpo en kg
paso=0.0001;    % paso temporal entre iteracion e iteracion, en segundos

%% Solucion analítica

%Encuentra parametros para graficos de posicion y velocidad
w=sqrt(g/l);
% Como A*cos(phi)=titaini y -wAsin(phi)=velini => -w*tg(phi)=velini/titaini
% Pero si hay condiciones iniciales con titaini = 0 la ec diverge hay que
% considerar el caso por separado, tambien se debe considerar el calculo de
% A por separado pues en el caso de posicion inicial cero no sirve la
% condicion de titaini para el calculo.
if titaini==0 
    phi=pi/2;
    A=velini/-w;
else
    phi=-atan((velini/titaini)/w);
    A=titaini/cos(phi);
end

%% Solucion numerica 
%% preparo las variables para la iteracion
tin=0;         %tiempo inicial
tfin=t;   %tiempo final
tiempo = tin:paso:tfin;   %vector de tiempos (0,paso,2paso,3paso,...) 
npasos = length(tiempo); %Genera los vectores vacios para que le asigne la memoria necesaria de entrada y no tenga que estar moviendo de lugar el vector cada vez q se agranda
velang = zeros(1,npasos);
teta= zeros(1,npasos);
teta(1) = titaini;
velang(1) = velini;
%%                MAIN LOOP 
%integracion numurica
for i = 2:npasos
  velang(i) = velang(i-1)-(((g/l)*sin(teta(i-1))))*paso;
  teta(i) = teta(i-1)+velang(i-1)*paso;
end;


%% Graficos de solucion analitica a partir de dinamica
titaAnalitica=A*cos(w*tiempo+phi);
velAnalitica=-w*A*sin(w*tiempo+phi); 


%% Muestra resultados
figure(1)

Titulo=['Paso utilizado: ',num2str(paso),' segundos'];
if Comparar
    plot(tiempo,teta,'b',tiempo,titaAnalitica,'g')
    legend('Numerica','Aproximación analítica')
    Subtitulo=['Comparación entre solución numerica y analitica aproximada para un angulo de ',num2str(titaini/pi*180),' grados.'];
    title ({Subtitulo;Titulo})
else
    plot(tiempo,teta,'b')
    legend('Numerica')
    title (Titulo)
end
xlabel ('Tiempo(s)')
ylabel ('Angulo(rad)')


%% Analisis calitativo de la forma de la funcion aproximada vs la numerica

% La idea es reescalar en la variable temporal para que los periodos
% coincidan y se pueda comparar la forma funcional

if Compararforma
    PeriodoAprox=2*pi/w;
    IndicePeriodoAprox=round(PeriodoAprox/paso);
    
    if length(teta)/IndicePeriodoAprox <3
        msgbox('No hay suficientes periodos para hacer el analisis');
    else
        [Max1,Pos1]=max(teta(1:IndicePeriodoAprox));
        [Max2,Pos2]=max(teta(IndicePeriodoAprox:2*IndicePeriodoAprox));
        [Max3,Pos3]=max(teta(2*IndicePeriodoAprox:3*IndicePeriodoAprox));
        
        Pos2=Pos2+IndicePeriodoAprox;
        Pos3=Pos3+2*IndicePeriodoAprox;
        
        Delta1=(Pos2-Pos1)*paso;
        Delta2=(Pos3-Pos2)*paso;
        
        if abs(Delta1-Delta2)>Delta1/3
            msgbox('No se logro encontrar bien el periodo de la solucion numerica');
        else
            Factor=Delta1/PeriodoAprox;
            figure(2)
            Titulo=['Paso utilizado: ',num2str(paso),' segundos'];
            plot(tiempo,teta,'b',tiempo*Factor,titaAnalitica,'g')
            legend('Numerica','Aproximación analítica')
            Subtitulo=['Comparación entre solución numerica y analitica aproximada para un angulo de ',num2str(titaini/pi*180),' grados, reescalando la variable temporal para que conicidan los periodos'];
            title ({Subtitulo;Titulo})
            xlabel ('Tiempo(s) - Reescalado en la solucion analitica')
            ylabel ('Angulo(rad)')
        end
    end
end
%% Analisis y comparacion de datos en base a un calculo energetico

% La idea de esta seccion es comparar la solucion energetica exacta (que solo
% permite calcular velocidades en funcion del angulo, con la solucion
% analitica aproximada y con la solucion numerica. 

if Velocidades

    figure(3) %Crea una nueva figura
    
    Titulo=['Paso utilizado: ',num2str(paso),' segundos'];
    Subtitulo=['Comparación entre solución numerica y analitica (aproximada y mediante energia) para un angulo de ',num2str(titaini/pi*180),' grados.'];

    %%Calculos a partir de los resultados numericos
    
    % Como la iteración numerica no conserva energia, y el problema diverge
    % solo tiene sentido tomar los datos para los titas menores que el
    % maximo teorico, porque sino la velocidad en los calculos exactos
    % daria una energia cinetica negativa. 
    
    %Calculo de la energia teorica del sistema a partir de las condiciones
    %iniciales
    hini=l*(1-cos(titaini)); % Calcula la altura inicial
    E=1/2*m*(velini*l)*(velini*l) + m*g*hini; %Calcula la energia inicial
    
    % Con la Energia inicial calculamos el maximo tita teorico posible:
    hmax=E/(m*g);
    TitaMax=acos(1-hmax/l);
    
    % Restringimos los datos numericos al rango de titas que nos interesa
    % comparar (no hace falta en realidad porque podemos independizar las
    % absisa de los diferentes graficos)
    %tetaRestringido=teta(abs(teta)<=TitaMax);
    %velangRestringido=velang(abs(teta)<=TitaMax);
    %velangRestringido=abs(velangRestringido); %Calcula el modulo porque a efectos de la energia es irrelevante si va o vuelve.
    
    %plot (tetaRestringido,velangRestringido,'b')
    plot (teta,abs(velang),'b')
    
    % Hacemos el mismo calculo en terminos de la energia
    Titas=-TitaMax:0.00001:TitaMax; %Genera el vector de angulos a graficarm nota, hay que poner un paso bien chico porque sino arrastra mucho error en los calculos siguientes
    htita=l*(1-cos(Titas)); %Genera el vector de anturas en funcion de tita;
    Ep=m*g*htita; %Genera el vector de energia potencial en funcion de tita
    Ec=E-Ep; %Genera el vector de energia cinetica en funcion de tita
    VelEnergiavsTita=sqrt(2/m*Ec)/l; %Genera el vector de velocidades calculadas mediante energia en funcion de tita, divide por l para que sea velocidad angular.

    hold on
    plot (Titas,VelEnergiavsTita,'r')

    % Hacemos el mismo grafico en terminos de el calculo analitico
    % aproximado
    hold on 
    plot (titaAnalitica,abs(velAnalitica),'g');
    
    legend('Calculo Numerico','Mediante energía', 'Mediante aproximación analítica')
    xlabel ('Angulo(rad)')
    ylabel ('Velocidad angular (rad/s)')
    
end