% With M1 = M2, J1 = J2, state plane analysis gives:
% alpha = 2*atan(M1/J1);
% beta = 2*atan(J1/(2+M1));
% pi/F = (alpha + beta);
% J = F/pi*2*M1;

clear all;

%% Analytical Solution
% Direct analytical solution for comparison
% Possible for SRC in this mode, but may not be possible for other
% modes/converters

% Plot control trajectory: J-vs-beta for varying F
beta = 0:.1:pi;
for F=1.25
    J = 2*F/pi*2.*tan(pi/2/F-beta/2).*tan(beta/2)./(1-tan(pi/2/F-beta/2).*tan(beta/2));
    
    figure(1); hold on;
    plot(beta,J, 'LineWidth',3)
end

figure(1)
xlabel('\beta')
ylabel('J')

%% Example Solution #1 - Sweep and check
% Pick one (or more) variables to sweep over a wide range.  This reduces
% the number of variables.  Then, use the "extra" equations resulting from
% the reduced number of variables to check and determine the valid point.

F = 1.25; %As an example, find curve for F=1.25
for beta = 0:.1:pi

    J1range = 0:.01:10; % Pick J1 as variable to sweep

    for i = 1:length(J1range)
        J1 = J1range(i);
        
        % Use J1 to solve three equations
        M1 = J1/tan(beta/2)-2; 
        alpha = 2*atan(M1/J1);
        J(i) = F/pi*2*M1;
        
        % Use fourth equation to check validity
        F_test(i) = pi/(alpha + beta);
    end

    %Figure 2 used to plot F_test to see which J1 gives the F at the correct
    %operating point, i.e. F_test = F
%     figure(2);
%     plot(J1range, F_test,'b', 'LineWidth',3);
%     hold on;
%     plot([0 10], [F F], ':r');
%     ylim([-10 10])
%     xlabel('J_1');
%     ylabel('F');

    % Find the answer (J) at the point where F_test = F
    Err = abs(F_test - F); % Error between F_test and F
    J = J(Err == min(Err)); % J at the point where F_test ~ F

    figure(1);
    plot(beta, J, 'dg'); % Plot results of sweeping with green diamonds
end

%% Example solution 2 using solver
% Use matlab solver to minimize equation error of a system of nonlinear
% equations
% type "doc fsolve" to see documentation and examples for this function

F = 1.25; %As an example, find curve for F=1.25
for beta = 0:.1:pi
    % Template of function implementing state plane equations as error
    % functions.  This function is defined at the bottom of this script
    % [ Err ] = SPEqs( alpha, beta, J1, M1, J, F )
    

    % Redefine function inputs to allow passing in constants (beta and F,
    % in this case).  All parameters that are solved are loaded into a
    % single vector per fsolve requirements
    myfun = @(x) SPEqs( x(1), beta, x(2), x(3), x(4), F );
    % so x = [alpha, J1, M1, J], with F and beta fixed to constants

    X0 = [pi/4, 1, 1,  1.05]; %Initial guess for the solver
    [X,FVAL,EXITFLAG] = fsolve(myfun,X0);

    if(EXITFLAG == 1) %If solver successfully converged
        
        % allocate results from the solver back to the variables        
        alpha = X(1);
        J1 = X(2);
        M1 = X(3);
        J = X(4);

        figure(1);
        plot(beta, J, 'or'); %Plot results of solver using red circles
    end

end

%% Check answer by plotting state plane
%Get a single point to check (re-use same solver)
figure(3); hold on;

%operating point to plot
beta = pi/4; F = 1.25;

myfun = @(x) SPEqs( x(1), beta, x(2), x(3), x(4), F );
[X,~,EXITFLAG] = fsolve(myfun,X0);
[alpha, J1, M1, J] = deal(X(1), X(2), X(3), X(4));

% Subinterbal 1
theta1 = linspace(pi/2 + alpha/2, pi/2 - alpha/2, 100); %range of angles 
% swept within alpha, referenced to the positive x-axis
r1 = sqrt(M1^2 + J1^2);
mr1 = r1*cos(theta1);
jl1 = r1*sin(theta1);

plot(mr1, jl1, 'r', 'LineWidth', 3);
plot(-mr1, -jl1, 'r', 'LineWidth', 3); %Subinterval III, (anti-symmetric)

% Subinterval II
theta2 = linspace(beta/2, -beta/2, 100);
r2 = sqrt((2+M1)^2 + J1^2);
mr2 = r2*cos(theta2) - 2; % "-2" because the center of resonance is not at the origin
jl2 = r2*sin(theta2);

plot(mr2, jl2, 'b', 'LineWidth',3);
plot(-mr2, -jl2, 'b', 'LineWidth', 3); %Subinterval III, (anti-symmetric)

xlabel('m_r');
ylabel('j_l');

%% Plot time-domain Waveforms
%vbase, Ibase, and wo are arbitrary for this example. For a real converter,
%these would come from the design and operating point.  To show
%denormalization, these numbers are just made up.
Vbase = 100;
Ibase = 10;
wo = 100e3;

%      I    II   III   IV
vr = [mr1, mr2, -mr1, -mr2]*Vbase;
il = [jl1, jl2, -jl1, -jl2]*Ibase;

% Because angles are all referenced to the positive x-axis, need to do some
% shifting to plot them time aligned.  
t = [(theta1 - theta1(1))*wo,  (theta1(end) - theta1(1))*wo + (theta2 - theta2(1))*wo];
t = -[t, t(end) + t];
Ts = t(end);

figure(4); hold on;
subplot(2,1,1)
plot(t/Ts, vr, 'LineWidth',3);
xlabel('t/T_s');
ylabel('v_r [V]');

subplot(2,1,2)
plot(t/Ts, il, 'LineWidth',3);
xlabel('t/T_s');
ylabel('i_l [A]');




%% State plane equations as error function
function [ Err ] = SPEqs( alpha, beta, J1, M1, J, F )
% State plane equations for the SRC with Vout = n*Vg
% Implemented as error equations, for use with a solver.  

Err(1) = alpha - 2*atan2(M1,J1);
Err(2) = beta - 2*atan2(J1,(2+M1));
Err(3) = pi/F - (alpha + beta);
Err(4) = J - F/pi*2*M1;


end