function [t,VAR,Output] = CoupledSecondOrderODE
%===========================================================================
% File: CoupledSecondOrderODE.m created Jan 20 2026 by MotionGenesis 6.6.
% Portions copyright (c) 2009-2026 Motion Genesis LLC. are licensed under
% the 3-clause BSD license.  https://opensource.org/licenses/BSD-3-Clause.
% This copyright notice must appear in all copies and distributions.
% MotionGenesis Professional Licensee: Motion Genesis LLC.
%===========================================================================
eventDetectedByIntegratorTerminate1OrContinue0 = [];
xDt=0; qDDt=0; qADDt=0; qBDDt=0; yDDt=0;


%-------------------------------+--------------------------+-------------------+-----------------
% Quantity                      | Value                    | Units             | Description
%-------------------------------|--------------------------|-------------------|-----------------
b                               =  0.0;                    % UNITS               Constant
k                               =  0.0;                    % UNITS               Constant
m                               =  1;                      % kg                  Constant

q                               =  0;                      % deg                 Initial Value
qA                              =  90;                     % deg                 Initial Value
qB                              =  1.0;                    % deg                 Initial Value
x                               =  1;                      % m                   Initial Value
y                               =  1;                      % m                   Initial Value
qDt                             =  2;                      % rad/sec             Initial Value
qADt                            =  0;                      % rad/sec             Initial Value
qBDt                            =  0;                      % rad/sec             Initial Value
yDt                             =  0;                      % m/sec               Initial Value

tInitial                        =  0.0;                    % second              Initial Time
tFinal                          =  10;                     % sec                 Final Time
tStep                           =  0.02;                   % sec                 Integration Step
printIntScreen                  =  1;                      % 0 or +integer       0 is NO screen output
printIntFile                    =  1;                      % 0 or +integer       0 is NO file   output
absError                        =  1.0E-07;                %                     Absolute Error
relError                        =  1.0E-08;                %                     Relative Error
%-------------------------------+--------------------------+-------------------+-----------------

% Unit conversions
DEGtoRAD = pi / 180.0;
RADtoDEG = 180.0 / pi;
q = q * DEGtoRAD;
qA = qA * DEGtoRAD;
qB = qB * DEGtoRAD;

VAR = SetMatrixFromNamedQuantities;
[t,VAR,Output] = IntegrateForwardOrBackward( tInitial, tFinal, tStep, absError, relError, VAR, printIntScreen, printIntFile );
OutputToScreenOrFile( [], 0, 0 );   % Close output files.
if( printIntFile ~= 0 ),  PlotOutputFiles;  end


%===========================================================================
function sys = mdlDerivatives( t, VAR, uSimulink )
%===========================================================================
SetNamedQuantitiesFromMatrix( VAR );
xDt = sin(t);

qDDt = cos(t) - sin(q);
yDDt = -(k*y+b*yDt)/m;

COEF = zeros( 2, 2 );
COEF(1,1) = 21 - sin(qB)^2;
COEF(2,2) = 1;
RHS = zeros( 1, 2 );
RHS(1) = 2*sin(qB)*cos(qB)*qADt*qBDt - 10*sin(qA);
RHS(2) = -sin(qB)*cos(qB)*qADt^2;
SolutionToAlgebraicEquations = COEF \ transpose(RHS);

% Update variables after uncoupling equations
qADDt = SolutionToAlgebraicEquations(1);
qBDDt = SolutionToAlgebraicEquations(2);

sys = transpose( SetMatrixOfDerivativesPriorToIntegrationStep );
end



%===========================================================================
function VAR = SetMatrixFromNamedQuantities
%===========================================================================
VAR = zeros( 1, 9 );
VAR(1) = q;
VAR(2) = qA;
VAR(3) = qB;
VAR(4) = x;
VAR(5) = y;
VAR(6) = qDt;
VAR(7) = qADt;
VAR(8) = qBDt;
VAR(9) = yDt;
end


%===========================================================================
function SetNamedQuantitiesFromMatrix( VAR )
%===========================================================================
q = VAR(1);
qA = VAR(2);
qB = VAR(3);
x = VAR(4);
y = VAR(5);
qDt = VAR(6);
qADt = VAR(7);
qBDt = VAR(8);
yDt = VAR(9);
end


%===========================================================================
function VARp = SetMatrixOfDerivativesPriorToIntegrationStep
%===========================================================================
VARp = zeros( 1, 9 );
VARp(1) = qDt;
VARp(2) = qADt;
VARp(3) = qBDt;
VARp(4) = xDt;
VARp(5) = yDt;
VARp(6) = qDDt;
VARp(7) = qADDt;
VARp(8) = qBDDt;
VARp(9) = yDDt;
end



%===========================================================================
function Output = mdlOutputs( t, VAR, uSimulink )
%===========================================================================
Output = zeros( 1, 10 );
Output(1) = t;
Output(2) = x;
Output(3) = xDt;

Output(4) = t;
Output(5) = q*RADtoDEG;
Output(6) = qDt*RADtoDEG;
Output(7) = qDDt;

Output(8) = t;
Output(9) = qA*RADtoDEG;
Output(10) = qB*RADtoDEG;
end


%===========================================================================
function OutputToScreenOrFile( Output, shouldPrintToScreen, shouldPrintToFile )
%===========================================================================
persistent FileIdentifier hasHeaderInformationBeenWritten;

if( isempty(Output) ),
   if( ~isempty(FileIdentifier) ),
      for( i = 1 : 3 ),  fclose( FileIdentifier(i) );  end
      clear FileIdentifier;
      fprintf( 1, '\n Output is in the files CoupledSecondOrderODE.i  (i=1,2,3)\n\n' );
   end
   clear hasHeaderInformationBeenWritten;
   return;
end

if( isempty(hasHeaderInformationBeenWritten) ),
   if( shouldPrintToScreen ),
      fprintf( 1,                '%%       t              x             x''\n' );
      fprintf( 1,                '%%   (seconds)         (m)          (m/sec)\n\n' );
   end
   if( shouldPrintToFile && isempty(FileIdentifier) ),
      FileIdentifier = zeros( 1, 3 );
      FileIdentifier(1) = fopen('CoupledSecondOrderODE.1', 'wt');   if( FileIdentifier(1) == -1 ), error('Error: unable to open file CoupledSecondOrderODE.1'); end
      fprintf(FileIdentifier(1), '%% FILE: CoupledSecondOrderODE.1\n%%\n' );
      fprintf(FileIdentifier(1), '%%       t              x             x''\n' );
      fprintf(FileIdentifier(1), '%%   (seconds)         (m)          (m/sec)\n\n' );
      FileIdentifier(2) = fopen('CoupledSecondOrderODE.2', 'wt');   if( FileIdentifier(2) == -1 ), error('Error: unable to open file CoupledSecondOrderODE.2'); end
      fprintf(FileIdentifier(2), '%% FILE: CoupledSecondOrderODE.2\n%%\n' );
      fprintf(FileIdentifier(2), '%%       t              q             q''             q''''\n' );
      fprintf(FileIdentifier(2), '%%   (seconds)      (degrees)      (deg/sec)     (rad/sec^2)\n\n' );
      FileIdentifier(3) = fopen('CoupledSecondOrderODE.3', 'wt');   if( FileIdentifier(3) == -1 ), error('Error: unable to open file CoupledSecondOrderODE.3'); end
      fprintf(FileIdentifier(3), '%% FILE: CoupledSecondOrderODE.3\n%%\n' );
      fprintf(FileIdentifier(3), '%%       t             qA             qB\n' );
      fprintf(FileIdentifier(3), '%%     (sec)        (degrees)      (degrees)\n\n' );
   end
   hasHeaderInformationBeenWritten = 1;
end

if( shouldPrintToScreen ), WriteNumericalData( 1,                 Output(1:3) );  end
if( shouldPrintToFile ),   WriteNumericalData( FileIdentifier(1), Output(1:3) );  end
if( shouldPrintToFile ),   WriteNumericalData( FileIdentifier(2), Output(4:7) );  end
if( shouldPrintToFile ),   WriteNumericalData( FileIdentifier(3), Output(8:10) );  end
end


%===========================================================================
function WriteNumericalData( fileIdentifier, Output )
%===========================================================================
numberOfOutputQuantities = length( Output );
if( numberOfOutputQuantities > 0 ),
   for( i = 1 : numberOfOutputQuantities ),
      fprintf( fileIdentifier, ' %- 14.6E', Output(i) );
   end
   fprintf( fileIdentifier, '\n' );
end
end



%===========================================================================
function PlotOutputFiles
%===========================================================================

figure;
data = load( 'CoupledSecondOrderODE.3' ); 
plot( data(:,1),data(:,2),'-b', data(:,1),data(:,3),'-.g', 'LineWidth',3 );
legend( 'qA (degrees)', 'qB (degrees)' );
xlabel('t (sec)');   % ylabel('Some y-axis label');   title('Some plot title');
clear data;
end



%===========================================================================
function [functionsToEvaluateForEvent, eventTerminatesIntegration1Otherwise0ToContinue, eventDirection_AscendingIs1_CrossingIs0_DescendingIsNegative1] = EventDetection( t, VAR, uSimulink )
%===========================================================================
% Detects when designated functions are zero or cross zero with positive or negative slope.
% Step 1: Uncomment call to mdlDerivatives and mdlOutputs.
% Step 2: Change functionsToEvaluateForEvent,                      e.g., change  []  to  [t - 5.67]  to stop at t = 5.67.
% Step 3: Change eventTerminatesIntegration1Otherwise0ToContinue,  e.g., change  []  to  [1]  to stop integrating.
% Step 4: Change eventDirection_AscendingIs1_CrossingIs0_DescendingIsNegative1,  e.g., change  []  to  [1].
% Step 5: Possibly modify function EventDetectedByIntegrator (if eventTerminatesIntegration1Otherwise0ToContinue is 0).
%---------------------------------------------------------------------------
% mdlDerivatives( t, VAR, uSimulink );        % UNCOMMENT FOR EVENT HANDLING
% mdlOutputs(     t, VAR, uSimulink );        % UNCOMMENT FOR EVENT HANDLING
functionsToEvaluateForEvent = [];
eventTerminatesIntegration1Otherwise0ToContinue = [];
eventDirection_AscendingIs1_CrossingIs0_DescendingIsNegative1 = [];
eventDetectedByIntegratorTerminate1OrContinue0 = eventTerminatesIntegration1Otherwise0ToContinue;
end


%===========================================================================
function [isIntegrationFinished, VAR] = EventDetectedByIntegrator( t, VAR, nIndexOfEvents )
%===========================================================================
isIntegrationFinished = eventDetectedByIntegratorTerminate1OrContinue0( nIndexOfEvents );
if( ~isIntegrationFinished ),
   SetNamedQuantitiesFromMatrix( VAR );
%  Put code here to modify how integration continues.
   VAR = SetMatrixFromNamedQuantities;
end
end



%===========================================================================
function [t,VAR,Output] = IntegrateForwardOrBackward( tInitial, tFinal, tStep, absError, relError, VAR, printIntScreen, printIntFile )
%===========================================================================
OdeMatlabOptions = odeset( 'RelTol',relError, 'AbsTol',absError, 'MaxStep',tStep, 'Events',@EventDetection );
t = tInitial;                 epsilonT = 0.001*tStep;                   tFinalMinusEpsilonT = tFinal - epsilonT;
printCounterScreen = 0;       integrateForward = tFinal >= tInitial;    tAtEndOfIntegrationStep = t + tStep;
printCounterFile   = 0;       isIntegrationFinished = 0;
mdlDerivatives( t, VAR, 0 );
while 1,
   if( (integrateForward && t >= tFinalMinusEpsilonT) || (~integrateForward && t <= tFinalMinusEpsilonT) ), isIntegrationFinished = 1;  end
   shouldPrintToScreen = printIntScreen && ( isIntegrationFinished || printCounterScreen <= 0.01 );
   shouldPrintToFile   = printIntFile   && ( isIntegrationFinished || printCounterFile   <= 0.01 );
   if( isIntegrationFinished || shouldPrintToScreen || shouldPrintToFile ),
      Output = mdlOutputs( t, VAR, 0 );
      OutputToScreenOrFile( Output, shouldPrintToScreen, shouldPrintToFile );
      if( isIntegrationFinished ), break;  end
      if( shouldPrintToScreen ), printCounterScreen = printIntScreen;  end
      if( shouldPrintToFile ),   printCounterFile   = printIntFile;    end
   end
   [TimeOdeArray, VarOdeArray, timeEventOccurredInIntegrationStep, nStatesArraysAtEvent, nIndexOfEvents] = ode45( @mdlDerivatives, [t tAtEndOfIntegrationStep], VAR, OdeMatlabOptions, 0 );
   if( isempty(timeEventOccurredInIntegrationStep) ),
      lastIndex = length( TimeOdeArray );
      t = TimeOdeArray( lastIndex );
      VAR = VarOdeArray( lastIndex, : );
      printCounterScreen = printCounterScreen - 1;
      printCounterFile   = printCounterFile   - 1;
      if( abs(tAtEndOfIntegrationStep - t) >= abs(epsilonT) ), warning('numerical integration failed'); break;  end
      tAtEndOfIntegrationStep = t + tStep;
      if( (integrateForward && tAtEndOfIntegrationStep > tFinal) || (~integrateForward && tAtEndOfIntegrationStep < tFinal) ) tAtEndOfIntegrationStep = tFinal;  end
   else
      t = timeEventOccurredInIntegrationStep( 1 );    % time  at firstEvent = 1 during this integration step.
      VAR = nStatesArraysAtEvent( 1, : );             % state at firstEvent = 1 during this integration step.
      printCounterScreen = 0;
      printCounterFile   = 0;
      [isIntegrationFinished, VAR] = EventDetectedByIntegrator( t, VAR, nIndexOfEvents(1) );
   end
end
end


%==============================================
end    % End of function CoupledSecondOrderODE
%==============================================
