%% SI_3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               DESCRIPTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%{
This script can be used to calculate angles between modular parts by
measuring the cable length from part to part. Measuring is done at a 
distance of 'a' from the pivot point. 
%}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               INPUTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%             a:            length in mm from pivot point to end of the rib
%             b:            length in mm from pivot point to pivot point
%             c:            thickness of the rib modular part
%             n:            number of modular parts excl baseplate (1-10)
%             i:            choose which iteration should be plotted from the experimental load
%             g:            if g is 1 one side is plotted, g = 2 both sides are plotted
%             m:            which modular parts should be plotted
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               OUTPUTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Plot the eigenvalues of the system as a function of the dynamic pressure
% parameter.
%                                   TBD
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               References
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               Authors
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Code Creaded: 02/27/2019
% Code Author: Pieter Wiersinga
% Tewente University, Enschede, The Netherlands
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                               UPDATES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% {Day/Month/Year}: {Name of updater}
% Enter your updates here:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Plots setting
close all; clear all; clc;
set(0,'DefaultAxesFontSize',16); set(0,'DefaultTextFontSize',16);
set(0,'DefaultLineLineWidth',1.5); set(0,'DefaultLineMarkerSize',5.5);
set(0,'DefaultAxesFontName','Sitka Subheading')%Helvetica - default
set(0,'defaultfigurecolor',[1 1 1]);
%%
figure(1), clf(1),hold on
N = 'Dynamic matlab model deflection with increasing external force 0 - 3.0 kg';
title(N,'FontSize',10)
axis equal; xlabel('X [mm]'); ylabel('Y [mm]')

a = 66;                     %51 length in mm from pivot point to end of the rib
b = 35;                     %30 length in mm from pivot point to pivot point
c = 10;                     %10 thickness of the rib modular part

% ------------------------- Input  -----------------------------
n = 5;                      %number of modular parts excl baseplate (1-10)
i = 7;                      %choose which iteration should be plotted from the experimental load
g = 2;                      %if g is 1 one side is plotted, g = 2 both sides are plotted
m = 3:n;                  %which modular parts should be plotted
% ------------------------- Script:  -----------------------------
%Measured ropelength between the ribs in mm (right),
% at a distance of 'a' from the pivot point along the right side of the rib.

g=g*i;          %determine settings for h for loop
for h= 1:g
    b = 35;
    if h <= g/2
        w=1;
        i=h;e='-or';
    else
        w=-1;
        i=h-(g/2);e='-ob';
    end
    if i == 1
        r1 =    24.8;
        r2 =    23.2;
        r3 =    25;
        r4 =    25;
        r5 =    24.5;
        r6 =    b-c;
    elseif i == 2
        r1 =    25;
        r2 =    20.8;
        r3 =    24;
        r4 =    24.1;
        r5 =    24.3;
        r6 =    b-c;
    elseif i == 3
        r1 =    25.3;
        r2 =    18.7;
        r3 =    22.6;
        r4 =    23;
        r5 =    24.6;
        r6 =    b-c;
    elseif i == 4
        r1 =    24.8;
        r2 =    16;
        r3 =    21.7;
        r4 =    23;
        r5 =    24.6;
        r6 =    b-c;
    elseif i == 5
        r1 =    24.9;
        r2 =    14.9;
        r3 =    20.3;
        r4 =    22.4;
        r5 =    23.8;
        r6 =    b-c;
    elseif i == 6
        r1 =    24;
        r2 =    13.8;
        r3 =    19.6;
        r4 =    22.7;
        r5 =    23.2;
        r6 =    b-c;
    elseif i == 7
        r1 =    24.4;
        r2 =    12.2;
        r3 =    18.6;
        r4 =    21.3;
        r5 =    23.5;
        r6 =    b-c;
    else
        r1 =    b-c;
        r2 =    b-c;
        r3 =    b-c;
        r4 =    b-c;
        r5 =    b-c;
        r6 =    b-c;
    end

    % --------------------- Determinining angles -----------------------------

    %converting measured ropelengths in vector format
    R = [r1,r2,r3,r4,r5,r6];

    %theta initiation
    theta = 0;                 %initiation theta in radians
    thetadegrees = 0;          %initiation theta in degrees
    thetavsr =0;               %initiation theta vs horizontal reference in radians

    b= b-c;

    %determining angles using goniometry and abc formula
    for k = 1:n+1
        %determination of direction of bending towards left d = 1 right d = -1
        if R(k) <= b
            d= -1*w  ;
        else
            d = 1*w  ;
        end
        s1 = (2*a.^2)+ (b.^2) - R(k).^2;         %calculation constant
        %abc formula constants
        sa = (4* a.^4)+ 4*(a.^2)*(b.^2);         %a
        sb = (-4* s1 * a.^2);                    %b
        sc = (s1.^2)- 4* (a.^2) * (b.^2);        %c
        sD = (sb.^2 - (4 * sa * sc));            %Determinant

        %abc formula angle calculation
        theta(k+1) = d * acos((-sb+sqrt(sD))./(2*sa));  %outcome of the abc solution in radians
        %theta2(k+1) = acos((-sb-sqrt(sD))./(2*sa));    %other (not needed) outcome of the abc solution
        thetadegrees(k+1) = (theta(k+1) *180 /pi);      %angle variation neutral line in degrees
        %theta2degrees(k+1) = (theta2(k+1) *180 /pi);   %angle variation neutral line in degrees
    end

    % --------------------- Neutral line definition   ------------------------

    x = 0;                            %initiation x coordinate base
    y = 0;                            %initiation y coordinate base

    for k = 1:n+1
        x(k+1) = x(k)-d*c*sin(thetavsr(k)) - d*b*sin(thetavsr(k));     %determine next x coordinate pivotpoint
        y(k+1) = y(k)+c*cos(thetavsr(k)) + b*cos(thetavsr(k));         %determine next y coordinate pivotpoint
        thetavsr(k+1) = thetavsr(k)+ d* theta(k+1);                    %determine next angle neutral line vs vertical
        plot(x,y,e,'Linewidth',2)
    end


    %--------------------- Ribs definition and plotting ---------------------
    for k = 1:n+1
        c1(k)= x(k)-a*cos(thetavsr(k));             %determine x coordinates rib ends left
        c2(k)= x(k)+a*cos(thetavsr(k));             %determine x coordinates rib ends right
        d1(k)= y(k)-d*a*sin(thetavsr(k));           %determine y coordinates rib ends left
        d2(k)= y(k)+d*a*sin(thetavsr(k));           %determine y coordinates rib ends right
        c3(k)= x(k)-a*cos(thetavsr(k))-d*c*sin(thetavsr(k));       %determine x coordinates rib ends left
        c4(k)= x(k)+a*cos(thetavsr(k))-d*c*sin(thetavsr(k));       %determine x coordinates rib ends right
        d3(k)= y(k)-d*a*sin(thetavsr(k))+c*cos(thetavsr(k));       %determine y coordinates rib ends left
        d4(k)= y(k)+d*a*sin(thetavsr(k))+c*cos(thetavsr(k));       %determine y coordinates rib ends right
        plot([c1(k),c2(k)],[d1(k),d2(k)],e,'Linewidth',2)          %plotting ribs
    end

    thetavsrdegrees = thetavsr*180/pi;              %calculation of angle vs horizontal reference in degrees
    Angle_degrees_to_reftotal(:,h) = thetavsrdegrees(1:n)';  %transverse vector
    Angle_degrees_to_prevtotal(:,h)= thetadegrees(1:n)';    %transverse vector
    pause (1)

end
hold off