%% Simulation Data
simu = simulationClass();               %Create the Simulation Variable
simu.simMechanicsFile = 'SB2Bt_H_v6c_tidy.slx';    %Location of Simulink Model File
% simu.mcrCaseFile='batch.mat';           % COMMENT OUT FOR NON-BATCH RUNS
simu.mode = 'normal';                   %Specify Simulation Mode ('normal','accelerator','rapid-accelerator')
simu.explorer='on';                     %Turn SimMechanics Explorer (on/off)
simu.solver = 'ode4';                   %simu.solver = 'ode4' for fixed step & simu.solver = 'ode45' for variable step 
simu.dt = 0.005; 						%Simulation time-step [s]
simu.rampTime =50;                       %Wave Ramp Time Length [s]
simu.startTime = 0;                     %Simulation Start Time [s]
simu.endTime=400;            %Simulation End bdcloseTime [s]
simu.b2b  = 0;                          %Body-to-body interactions
simu.rho = 1025;                        %Water density - default = 1000 ! 
simu.nlHydro = 0;                       %Non-linear FK force
simu.CITime=100;
simu.paraview=0;                        %Output files for visualisation in Paraview (0-No ; 1-Yes)
simu.domainSize=50;                     %Side-length of the square domain for viewing
simu.dtOut = 0.1;

if simu.paraview==1
    waves.viz.numPointsX=50;
    waves.viz.numPointsY=5;
end
%% Wave Information 
%% noWaveCIC, no waves with radiation CIC  
% waves = waveClass('noWaveCIC');         %Create the Wave Variable and Specify Type      

%% Regular Waves  
% waves = waveClass('regularCIC');        %Create the Wave Variable and Specify Type                               
% waves.H = 2.75;                          %Wave Height [m]
% waves.T = 13;                            %Wave Period [s]

%% Irregular Waves using PM Spectrum with Convolution Integral Calculation
waves = waveClass('irregular');        %Create the Wave Variable and Specify Type
waves.H = 1.75;                        %Significant Wave Height [m]
waves.T = 13;                          %Peak Period [s]
waves.waveDir=0;                       %Wave Direction [deg]  
waves.spectrumType = 'BS';             %Wave Spectrum Type 
waves.phaseSeed=30;                %Wave Seed 

%% Irregular Waves using User-Defined Spectrum
% waves = waveClass('irregularImport');  %Create the Wave Variable and Specify Type
% waves.spectrumDataFile = 'ndbcBuoyData.txt';  %Name of User-Defined Spectrum File [2,:] = [omega, Sf]

%% User-Defined Time-Series
% waves = waveClass('userDefined');     %Create the Wave Variable and Specify Type
% waves.etaDataFile = 'umpqua46229_6_2008.mat'; % Name of User-Defined Time-Series File [:,2] = [time, wave_elev]

%% %%%%%%% --------- Body Data -----------%%%%%%%%%%%%%%
% Float
% Create the body(1) Variable, Set Location of Hydrodynamic Data File and Body Number Within this File.  
body(1) = bodyClass('.\hydro\wec4.h5');                    %Location of h5 file giving hydrodynamic data
body(1).hydroDataBodyNum = 1; 
body(1).mass = 1.85e+05;                                    %'equilibrium' if mass=displaced mass;                   
body(1).momOfInertia = [ 8.8e6 5.3e6 8.8e6];                %Moment of Inertia [kg*m^2]     
body(1).geometryFile = '.\geometry\wec4_float.stl';         %Location of Geometry File

% Reactor
body(2) = bodyClass('.\hydro\wec4.h5');                    %Location of h5 file giving hydrodynamic data
body(2).hydroDataBodyNum = 2;
body(2).mass = 1.3813e+07;                                  %'equilibrium' if mass=displaced mass;                   
body(2).momOfInertia = [2.91e9 3.09e9 5.94e9];
body(2).geometryFile = '.\geometry\wec4_reactor.stl'; 


%% %%%%%%%%%%% -------- Constraint Parameters ---------- %%%%%%%%%%%%%%%%%%
% Generic Corner PTO subsystems properties
StiffnessCorner=0;   % Stiffness used in each corner PTO subsystem [N/m]
DampingCorner=0;%1e6;   % Damping used in each corner PTO substystem [N/(m/s)]
PreloadPTOCorner=0;%-1.6113e+07/sind(50.4517874)/4;%0;
%%%%%%%% Definition of constraints location
% ----------------- Global Coordinate System  (m, m, m) --------------%
% Tether line attachemnt point on float in global frame
F1 = [4.243 2.375 -12.243];     % Corner tethers
F2 = [4.243 -2.375 -12.243];
F3 = [-4.243 -2.375 -12.243];
F4 = [-4.243 2.375 -12.243];
FC = [0 0 -14];                 % Central tether
% Tether line attachemnt point on reactor in global frame
T1 = [16.075 16.075 -34.165];   % Corner tethers
T2 = [16.075 -16.075 -34.165];
T3 = [-16.075 -16.075 -34.165];
T4 = [-16.075 16.075 -34.165];
TC = [0 0 -34.165];             % Central tether

% Rigid transform (rotation of xy-plane to align x-axis with horizontal projection of tether line)
PTOrotation = atan((T1(2)-F1(2))/(T1(1)-F1(1)));                            % PTO tether line [rad]
ptorotation =[PTOrotation, -PTOrotation, -pi+PTOrotation, pi-PTOrotation];
% Angle between xy-plant and tether line
alfaXYT1F1 = abs(atan((T1(3)-F1(3))/((T1(1)-F1(1))^2+(T1(2)-F1(2))^2)^0.5));% PTO tether line [rad]
% PTO and mooring orientation (in coordinate system rotated by rigid transform)
PTOorientation = [-cos(alfaXYT1F1) 0 sin(alfaXYT1F1)];                      

% --------------------- Local Coordinate System (m,m,m) ------------------%
% PTO tether coordinates in frame rotated by rigid transform
F1t1 = [F1(1)*cos(ptorotation(1))+F1(2)*sin(ptorotation(1)) -F1(1)*sin(ptorotation(1))+F1(2)*cos(ptorotation(1)) F1(3)]; 
T1t1 = [T1(1)*cos(ptorotation(1))+T1(2)*sin(ptorotation(1)) -T1(1)*sin(ptorotation(1))+T1(2)*cos(ptorotation(1)) T1(3)];
F1t2 = [F1(1)*cos(ptorotation(2))-F1(2)*sin(ptorotation(2)) -F1(1)*sin(ptorotation(2))-F1(2)*cos(ptorotation(2)) F1(3)];
T1t2 = [T1(1)*cos(ptorotation(2))-T1(2)*sin(ptorotation(2)) -T1(1)*sin(ptorotation(2))-T1(2)*cos(ptorotation(2)) T1(3)];
F1t3 = [-F1(1)*cos(ptorotation(3))-F1(2)*sin(ptorotation(3)) F1(1)*sin(ptorotation(3))-F1(2)*cos(ptorotation(3)) F1(3)];
T1t3 = [-T1(1)*cos(ptorotation(3))-T1(2)*sin(ptorotation(3)) T1(1)*sin(ptorotation(3))-T1(2)*cos(ptorotation(3)) T1(3)];
F1t4 = [-F1(1)*cos(ptorotation(4))+F1(2)*sin(ptorotation(4)) F1(1)*sin(ptorotation(4))+F1(2)*cos(ptorotation(4)) F1(3)];
T1t4 = [-T1(1)*cos(ptorotation(4))+T1(2)*sin(ptorotation(4)) T1(1)*sin(ptorotation(4))+T1(2)*cos(ptorotation(4)) T1(3)];
F1t5 = FC;
T1t5 = TC;

% Visualisation of PTOs (also needed for numerical stability of the PTO subsystem)
anglePTOvisu =  -(pi/2-alfaXYT1F1);       % Rotation of PTO visulaisation body
CablePTOcorner1 = [T1t1; F1t1];
CablePTOcorner2 = [T1t2; F1t2];
CablePTOcorner3 = [T1t3; F1t3];
CablePTOcorner4 = [T1t4; F1t4];
CablePTOcentral = [T1t5; F1t5];
bcablePTOcorner1 = mean(CablePTOcorner1);  
bcablePTOcorner2 = mean(CablePTOcorner2);
bcablePTOcorner3 = mean(CablePTOcorner3);
bcablePTOcorner4 = mean(CablePTOcorner4);
bcablePTOcentral = mean(CablePTOcentral);
% Vector giving centre of gravity of tether line visualisation body
bPTO = [bcablePTOcorner1; bcablePTOcorner2; bcablePTOcorner3; bcablePTOcorner4; bcablePTOcentral]; 


%%%%%%% --------------- Floating reactor  ---------%%%%%%%%%%%%%%%%%%%%                                 
constraint(1) = constraintClass('Floating 6 DoF');         
constraint(1).loc=[0 0 0];

%%%%%%% ---------------- Central PTO  -------------%%%%%%%%%%%%%%%%%%%%                 
%%%% ---- Rotational joints 
% Universal joint
constraint(2) = constraintClass('UJCentral');         % PTO Damping [N/(m/s)]
constraint(2).loc = T1t5;
% Gimbal
top_const_k=0;                                        % same for all gimbal in this model (only declared once)
top_const_c=0;                                        % same for all gimbal in this model (only declared once)
constraint(3) = constraintClass('GimbalCentral');     % PTO Damping [N/(m/s)]
constraint(3).loc = F1t5;
%%%% Central PTO visu
body(3) = bodyClass(''); 
body(3).mass = 0.01;					%'equilibrium' if mass=displaced mass;                   
body(3).momOfInertia = [1 1 1];
body(3).nhBody=1;                               	% Non-hydrodynamic body switch (0-No; 1-Yes)
body(3).cg=bPTO(5,:);
body(3).dispVol=0;                                      % Displaced volume [m^3]
body(3).geometryFile = '.\geometry\pto_visu.stl';

%%%% ---------------- Side PTO P1F1  ------------%%%%%%%%%%%%%%%%%%%%%%%                 
%%%% ---- Rotational joints 
% Universal joint
constraint(4) = constraintClass('UJP1');                                    % PTO Damping [N/(m/s)]
constraint(4).loc = T1t1;                                                   % location rotated by 'rigid transform P1F1'
% Gimbal
top_const_k=0;                                                              % same for all gimbal in this model (only declared once)
top_const_c=0;                                                              % same for all gimbal in this model (only declared once)
constraint(5) = constraintClass('GimbalF1');                                % PTO Damping [N/(m/s)]
constraint(5).loc = F1t1;                                                   % location rotated by 'rigid transform P1F1'
% P1F1 tether - Translational PTO
pto(1) = ptoClass('PTO1');                                                  % Create PTO Variable and Set PTO Name
pto(1).k=StiffnessCorner;                       % PTO Stiffness [N/m]
pto(1).c=DampingCorner;                         % PTO Damping [N/(m/s)]
pto(1).loc = T1t1;                              % PTO Location [m]
pto(1).orientation.z=PTOorientation;            % PTO orientation
SidePTO1Preload=PreloadPTOCorner;               % PTO tether preload [N]
%%%% P1F1 PTO visu
body(4) = bodyClass(''); 
body(4).mass = 0.01;                                                        %'equilibrium' if mass=displaced mass;                   
body(4).momOfInertia = [1 1 1];
body(4).nhBody=1;
body(4).cg=bPTO(1,:);                                                       % middle point of the tether
body(4).dispVol=0;
body(4).geometryFile = '.\geometry\pto_visu.stl';
body(4).initDisp = struct(...                                               % Structure defining the initial displacement
         'initLinDisp',          [0 0 0], ...                               % Initial displacement of center fo gravity - used for decay tests (format: [displacment in m], default = [0 0 0])
         'initAngularDispAxis',  [0 1 0], ...                               % Initial displacement of cog - axis of rotation - used for decay tests (format: [x y z], default = [1 0 0])
         'initAngularDispAngle', anglePTOvisu); % [rad]

%%%% ---------------- Side PTO P2F2  ------------%%%%%%%%%%%%%%%%%%%%%%%                 
%%%% ---- Rotational joints 
% Universal joint
constraint(6) = constraintClass('UJP2');        % PTO Damping [N/(m/s)]
constraint(6).loc = T1t2;                       % location rotated by 'rigid transform P2F2'
% Gimbal
constraint(7) = constraintClass('GimbalF2');    % PTO Damping [N/(m/s)]
constraint(7).loc = F1t2;                       % location rotated by 'rigid transform P2F2'
% P2F2 tether
pto(2) = ptoClass('PTO2');                      % Create PTO Variable and Set PTO Name
pto(2).k=StiffnessCorner;                       % PTO Stiffness [N/m]
pto(2).c=DampingCorner;                         % PTO Damping [N/(m/s)]
pto(2).loc = T1t2;                              % PTO Location [m]
pto(2).orientation.z=PTOorientation;            % PTO orientation
SidePTO2Preload=PreloadPTOCorner;               % PTO tether preload [N]
%%%% P2F2 PTO visu
body(5) = bodyClass(''); 
body(5).mass = 0.01;                            %'equilibrium' if mass=displaced mass;                   
body(5).momOfInertia = [1 1 1];
body(5).nhBody=1;
body(5).cg=bPTO(2,:);                           % middle point of the tether
body(5).dispVol=0;
body(5).geometryFile = '.\geometry\pto_visu.stl';
body(5).initDisp = struct(...                               % Structure defining the initial displacement
         'initLinDisp',          [0 0 0], ...               % Initial displacement of center fo gravity - used for decay tests (format: [displacment in m], default = [0 0 0])
         'initAngularDispAxis',  [0 1 0], ...               % Initial displacement of cog - axis of rotation - used for decay tests (format: [x y z], default = [1 0 0])
         'initAngularDispAngle', anglePTOvisu); % [rad]

%%%% ---------------- Side PTO P3F3  ------------%%%%%%%%%%%%%%%%%%%%%%%                 
%%%% ---- Rotational joints 
% Universal joint
constraint(8) = constraintClass('UJP3');                    % PTO Damping [N/(m/s)]
constraint(8).loc = T1t3;                                   % location rotated by 'rigid transform P3F3'
% Gimbal
constraint(9) = constraintClass('GimbalF3');                % PTO Damping [N/(m/s)]
constraint(9).loc =  F1t3;                                  % location rotated by 'rigid transform P3F3'
% P3F3 tether
pto(3) = ptoClass('PTO3');                                  % Create PTO Variable and Set PTO Name
pto(3).k=StiffnessCorner;                       % PTO Stiffness [N/m]
pto(3).c=DampingCorner;                         % PTO Damping [N/(m/s)]
pto(3).loc = T1t3;                              % PTO Location [m]
pto(3).orientation.z=PTOorientation;            % PTO orientation
SidePTO3Preload=PreloadPTOCorner;               % PTO tether preload [N]
%%%% P3F3 PTO visu
body(6) = bodyClass(''); 
body(6).mass = 0.01;                                                        %'equilibrium' if mass=displaced mass;                   
body(6).momOfInertia = [1 1 1];
body(6).nhBody=1;
body(6).cg=bPTO(3,:);                                                       % middle point of the tether
body(6).dispVol=0;
body(6).geometryFile = '.\geometry\pto_visu.stl';
body(6).initDisp = struct(...                                               % Structure defining the initial displacement
         'initLinDisp',          [0 0 0], ...                               % Initial displacement of center fo gravity - used for decay tests (format: [displacment in m], default = [0 0 0])
         'initAngularDispAxis',  [0 1 0], ...                               % Initial displacement of cog - axis of rotation - used for decay tests (format: [x y z], default = [1 0 0])
         'initAngularDispAngle', anglePTOvisu); % [rad]
     
%%%% ---------------- Side PTO P4F4  ------------%%%%%%%%%%%%%%%%%%%%%%%                 
%%%% ---- Rotational joints 
% Universal joint
constraint(10) = constraintClass('UJP4');      % PTO Damping [N/(m/s)]
constraint(10).loc =  T1t4;                    % location rotated by 'rigid transform P4F4'
% Gimbal
constraint(11) = constraintClass('GimbalF4');  % PTO Damping [N/(m/s)]
constraint(11).loc =  F1t4;                    % location rotated by 'rigid transform P4F4'
% P4F4 tether
pto(4) = ptoClass('PTO4');                      % Create PTO Variable and Set PTO Name
pto(4).k=StiffnessCorner;                      	% PTO Stiffness [N/m]
pto(4).c=DampingCorner;                     	% PTO Damping [N/(m/s)]
pto(4).loc =  T1t4;                             % PTO Location [m]
pto(4).orientation.z=PTOorientation;            % PTO orientation
SidePTO4Preload=PreloadPTOCorner;               % PTO tether preload [N]
%%%% P4F4 PTO visu
body(7) = bodyClass(''); 
body(7).mass = 0.01;                                    %'equilibrium' if mass=displaced mass;                   
body(7).momOfInertia = [1 1 1];
body(7).nhBody=1;
body(7).cg=bPTO(4,:);                                   % middle point of the tether
body(7).dispVol=0;
body(7).geometryFile = '.\geometry\pto_visu.stl';
body(7).initDisp = struct(...                           % Structure defining the initial displacement
         'initLinDisp',          [0 0 0], ...           % Initial displacement of center fo gravity - used for decay tests (format: [displacment in m], default = [0 0 0])
         'initAngularDispAxis',  [0 1 0], ...           % Initial displacement of cog - axis of rotation - used for decay tests (format: [x y z], default = [1 0 0])
         'initAngularDispAngle', anglePTOvisu); 	% [rad]   

%%%% back to declaration of pto(5) only possible after declaration of
%%%% pto(1,2,3,4)
% Central tether
pto(5) = ptoClass('CENTRAL TETHER');          	% Create PTO Variable and Set PTO Name
pto(5).k=5e5;                                   % Central Tether Stiffness [N/m]
pto(5).c=0;                                 	% Central Tether Damping [N/(m/s)]
pto(5).loc = T1t5;                              % Central Tether Location [m]
CentralPTOPreload=-1.6113e+07;                   % Central Tether preload [N]

%%%%%%%%%% --------- Linearised Mooring Model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mooring(1) = mooringClass('mooring');
mooring(1).ref=[0 , 0 , -34.165];
Kmoor=[2.0e4, 2.0e4 , 1.0e4 , 0.0 , 0.0 , 0.0 ];
Cmoor=[0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0 ];

mooring(1).matrix = struct(...                          % Structure defining damping, stiffness, and pre-tension.
                        'c',          diag(Cmoor), ...              % Mooring damping, 6 x 6 matrix. 
                        'k',          diag(Kmoor), ...              % Mooring stiffness, 6 x 6 matrix.
                        'preTension', [0 0 -6.302e+06 0 0 0]);