// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
funcprot(0);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

cv=%pi/180; // From degree to radian

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Latitude=52; // [°] 52 0 90

L=67; // Length of pendulum
M=28;

// Initial data

gamma0g=40; // [°] 40
chi0g=0; // [°]
// Initial position of pendulum

gammaDot0g=0; // [°/s] 0 10
chiDot0g=1; // [°/s] 0 1 5    
// Initial pendulum velocity

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

g=9.80616; // g45

omega=7.292115E-5;
// omega=7.292115E-5 * 100;
// omega=0;

theta=Latitude*cv;

gamma0=gamma0g*cv; // [°]
chi0=chi0g*cv;      // [°]

gammaDot0=gammaDot0g*cv; // [°/s]
chiDot0=chiDot0g*cv;     // [°/s]   

Lchi0=chiDot0*(sin(gamma0))^2

// disp(Lchi0);

Etot0=0.5*M*L^2*(chiDot0^2*(sin(gamma0))^2+gammaDot0^2)-g*L*cos(gamma0);

// disp(Etot);

t0=0;
tend=64; 
steps=6413; // 6413 // 643

// #####################################################################
// #####################################################################

function rkart=fkart(gamm,chi)
    x(1)= L*sin(gamm)*cos(chi);
    x(2)= L*sin(gamm)*sin(chi);
    x(3)=-L*cos(gamm);
    
    rkart=x;
endfunction

// kartData=fkart(45*cv,10*cv)
// disp(kartData);

// #####################################################################

function rVkart=fVkart(gamm,chi,gammaDot,chiDot)
    
    v(1)=L*(-chiDot*sin(gamm)*sin(chi)+gammaDot*cos(gamm)*cos(chi));
    v(2)=L*( chiDot*sin(gamm)*cos(chi)+gammaDot*cos(gamm)*sin(chi));
    v(3)=L*(                           gammaDot*sin(gamm));
    
    rVkart=v;
endfunction

// #####################################################################

function rvDot=fvDot(x,v)
    // return ph data
    
    gamm=x(1);
    chi=x(2);
    gammaDot=v(1);
    chiDot=v(2);
    
    b12_c=cos(theta)*sin(chi)-sin(theta)*cotg(gamm);
    b1_c= 2*omega*chiDot*(sin(gamm))^2*b12_c;
    b2_c=-2*omega*gammaDot*b12_c;
     
    b(1)=0.5*chiDot^2*sin(2*gamm)-(g/L)*sin(gamm)+b1_c;
    b(2)=-2*gammaDot*chiDot*cotg(gamm)+b2_c;

    rvDot(1:2)=v;
    rvDot(3:4)=b;
endfunction

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

function rPendulum=fPendulum(t,ph)
    // returns 'phDot data'
    x=ph(1:2);
    v=ph(3:4);
    rPendulum=fvDot(x,v);
endfunction;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

ph0(1)=gamma0;
ph0(2)=chi0;
ph0(3)=gammaDot0;
ph0(4)=chiDot0;

tData=linspace(t0,tend,steps);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

disp("### Starte ODE ...");

phData=ode(ph0,t0,tData,fPendulum);

disp("##> ODE fertig.");

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

to_deg=180/%pi;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

my_handle=scf(1);
clf(my_handle,"reset");
a=gca();

drawlater();
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
xtitle ( " Pendelbewegung - Orte der Pendelmasse" ,..
         " x (E) ",..
         " y (N) ",..
         " z (U) " );

//xtitle ( " Pendelbewegung - Blick von unten" ,..
//         " x (E) ",..
//         " y (N) " );

//xtitle ( " Pendelbewegung - Orte der Pendelmasse gegen Zeit" ,..
//         " t ",..
//         " x(E) " );

//xtitle ( " Änderungsgeschwindigkeit von Gamma gegen Zeit" ,..
//         " t ",..
//         " GammaDot [rad/s] " );

//xtitle ( " Azimuthwinkel der Pendelmasse gegen Zeit" ,..
//         " t ",..
//         " Chi [°] " );

//xtitle ( " Phasendiagramm:  Auslenkwinkel - Änderungsgeschwindigkeit " ,..
//         " Gamma [°] ",..
//         " GammaDot [°/s] " );
         
//xtitle ( " Phasendiagramm:  Azimuthwinkel - Änderungsgeschwindigkeit " ,..
//         " Chi [°] ",..
//         " ChiDot [°/s] " );

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

a.labels_font_size=3;
a.x_label.font_size = 3;
a.y_label.font_size = 3;
a.title.font_size = 3;

posData=[];
velData=[];

bc_gammaData=[];
bc_chiData=[];

D_chiData=[];

chi_DataY=[];
chi_DataX=[];

LpolData=[];

EtotData=[];

nbpoints=size(phData,"c");

if (nbpoints<>length(tData)) then
    disp("#!! Länge nicht stimmig - Abbruch");
    abort;
end

disp("### Starte Berechnungen ...");

chi_DataIx=1;

for Ix=1:nbpoints
    
    gamm=phData(1,Ix);
    chi=phData(2,Ix);
    gammaDot=phData(3,Ix);
    chiDot=phData(4,Ix);
    
    posData(1:3,Ix)=fkart(gamm,chi);
    
//    velData(1:3,Ix)=fVkart(gamm,chi,gammaDot,chiDot);
//    
//    velData(4,Ix)=sqrt((velData(1,Ix))^2+..
//                       (velData(2,Ix))^2+..
//                       (velData(3,Ix))^2);
    
    bc_0=cos(theta)*sin(gamm)*sin(chi)-sin(theta)*cos(gamm);
    
    bc_gammaData(Ix)= 2*omega*L*chiDot*sin(gamm)*bc_0;
    bc_chiData(Ix)  =-2*omega*L*gammaDot*bc_0;
    
//    D_chiData(Ix)=-L*M*bc_gammaData(Ix)
//    
//    EtotData(Ix)=0.5*M*L^2*(chiDot^2*(sin(gamm))^2+..
//                 gammaDot^2)-g*M*L*cos(gamm);
                 
//    chig=asind(sin(chi));
//    if (chig>0) then
//        chi_DataX(chi_DataIx)=Ix;
//        chi_DataY(chi_DataIx)=chig;
//        chi_DataIx=chi_DataIx+1;
//    end
    
    // LpolData(Ix)=abs(chiDot+omega)*(sin(gamm))^2;
    // theta=%pi/2
end

// Import function
//exec("C:\Users\Bernd\Werkbank\SciLab\FoucaultschesPendel\Main\polyfit.sci",-1);
//[Pn] = polyfit(chi_DataX,chi_DataY,1,GraphChk='Y');

//exec("C:\Users\Bernd\Werkbank\SciLab\FoucaultschesPendel\Main\polyfit.sci",-1);
//[Pn] = polyfit(tData,bc_gammaData,1,GraphChk='Y');

//exec("C:\Users\Bernd\Werkbank\SciLab\FoucaultschesPendel\Main\polyfit.sci",-1);
//[Pn] = polyfit(tData,bc_chiData,1,GraphChk='Y');

//disp(Pn);

disp("##> Berechnungen fertig.");

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

disp("### Starte Ausgaben ...");

param3d1(posData(1,:)', posData(2,:)', posData(3,:)');
// param3d1(velData(1,:)', velData(2,:)', velData(3,:)');

// subplot(211);
// plot(tData,posData(2,:)');
// plot(tData,posData(3,:)');

// subplot(211);
// disp(phData(3,:)'*to_deg);
// plot(tData,phData(3,:)');
// subplot(212);
// plot(tData,velData(4,:)');

// plot(tData,phData(4,:)');

// subplot(212);
// plot(tData,EtotData);

//xtitle ( " Coriolisbeschleunigung - Gammakomponente gegen Zeit " ,..
//         " t ",..
//         " bC_Gamma " );
// subplot(211);
// plot(tData,bc_gammaData);

//xtitle ( " Coriolisbeschleunigung - Chikomponente gegen Zeit " ,..
//         " t ",..
//         " bC_Chi " );
// subplot(212);
// plot(tData,bc_chiData);

// plot(tData,velData(3,:)');
// plot(tData,velData(4,:)');

// xtitle ( " Konstante der Bewegung am Pol " ,..
//         " t ",..
//         " CPol " );
// plot(tData,LpolData);
// disp(min(LpolData));
// disp(max(LpolData));

// plot(tData,D_chiData);
//disp(D_chiData);

//plot(tData,chi_Data,'.');

// plot(posData(1,:)',posData(2,:)'); // &&& $$$ %%%

// plot(posData(1,580:639)',posData(2,580:639)','r');

// plot2d(phData(1,1:80)'*to_deg,phData(3,1:80)'*to_deg);

// plot2d(phData(2,1:80)'*to_deg,phData(4,1:80)'*to_deg);
// plot2d(phData(1,:)'*to_deg,phData(3,:)'*to_deg);

// plot2d(phData(2,:)',phData(4,:)');

// disp(posData(2,:)');

// disp(phData(1,:)');
// disp(bc_chiData);

// disp(2*%pi/sqrt(g/L));

// disp(max(EtotData));
// disp(min(EtotData));
// disp(omega*tend*to_deg);

// disp(chi_DataIx-1);
// disp(chi_DataX(chi_DataIx-1));
// disp(chi_DataY(chi_DataIx-1));
// disp(Pn);

drawnow();

disp("##> Ausgaben fertig.");

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++