                  %%%%%% Written by Sarvenaz Memarzadeh 7/11/16 %%%%%%%%
clc
clear all
close all

%% Constant Parameters
ls = 0.065; % Solenoid's effective length (cm)
L = 0.1; % Separation between solenoid (cm)
sig = pi/6; % Phase advances per period

%% Trace of unit cell matrix (solved symbolically)
syms theta
M = [cos(theta) (ls/theta)*sin(theta)
    -(theta/ls)*sin(theta) cos(theta)];
Tr_M = sum(diag(M * [1 L; 0 1]))

%% Solution of trascendental equation
% Define function of theta
Fun = @(theta) (2*cos(theta) - L*theta*sin(theta)/ls)/2 - cos(sig);
theta = 0.5; % intial guess
r1 = fzero(Fun, theta) % finding the root
theta = r1;
a = cos(theta);
b = L*cos(theta)+ (ls*sin(theta))/theta;
c = -theta*sin(theta)/ls;
d = cos(theta)- L*theta*sin(theta)/ls;
%% Three Rays
v = [0.0012 ; 0];
t = [0.00095; 0.0075];
u = [0.0013 ; -0.005];

n = [0:35];
for ii = 1: length(n)
    Rl = [a b; c d]^n(ii);
    vl(:,ii)  = Rl * v;
    tl(:,ii) = Rl * t;
    ul(:,ii) = Rl * u;
end

figure
plot(n, vl(1,:) , 'rs-'); hold on;
plot(n, tl(1,:), 'b--'); hold on;
plot(n, ul(1,:), 'go-'); hold on;
legend ('(vl_n)_{0,0}' , '(tl_n)_{0,0}', '(ul_n)_{0,0}')
grid on; xlabel('n'); ylabel('r (m)')

