function[F,C,Lambda] = GPRForTernaryMRS_OptimalEqualizer(ND1,ND2,L,ChannelIndex)
% ----------------USAGE--------------------
% [F,C,Lambda] = GPRForTernaryMRS_OptimalEqualizer(ND1,ND2,L,ChannelIndex);
%
% ------------INPUT ARGUMENTS--------------
% ND1: Normalized density for upper channel
% ND2: Normalized density for lower channel
% L: Length of the channel targets (all targets are of the same length)
% ChannelIndex = 0 => Peak amplitude of X22 (temperature modulated) response is (ND1/ND2) times of peak amplitude of X11
% ChannelIndex = 1 => Peak amplitude of X22 (temperature modulated) response is same as that of X11

%%%%%%%%%%%%%%%%%% User Parameters %%%%%%%%%%%%%%%%%%%%
w1 = 69;
w2 = (ND2/ND1)*w1;
T = w1/ND1;
FIRChannelLength = 101; % Should be an odd number
N = 10; % Length of FIR equalizer filter = 2N+1
fprintf(1,'PW50 of unmodulated signal = %f nm\n',w1);
fprintf(1,'PW50 of modulated signal = %f nm\n',w2);
fprintf(1,'T = %f\n',T);
%%%%%%%%%%%% ENCODER: Generate input sequence 'a' %%%%%%%%%%%%%

% Generate binary sequence and map it to ternary sequence %
% BinarySeq = rand(1,60000);
% BinarySeq(find(BinarySeq > 0.5))=1;
% BinarySeq(find(BinarySeq <= 0.5))=0;
% Map binary to ternary sequence
% fprintf(1,'Mapping binary input to ternary...\n');
% [a] = MapBinaryToTernary(BinarySeq);

% Generate equiprobable ternary sequence
% a = rand(1,40000);
% a(find(a<0.33))=0;
% a(find(a>=0.33 & a<0.66))=1;
% a(find(a >= 0.66 & a<1))=2;
% 
% % Assign direction of magnetization
% j=+1;
% for i = 1:length(a)
%     if(a(i)~=0)
%         a(i)=a(i)*j;
%         j = j*-1;
%     end
% end
% % Separate into two sequences
% a1 = a;
% a2 = a;
% a1(find(abs(a1)==2)) = 0; % Sequences with 0, +1, -1
% a2(find(abs(a2)==1)) = 0; % Sequences with 0, +2, -2
% a2 = a2./2;

%%% Generate independent input sequences a1 and a2 %%%
a1 = rand(1,200000);
a1(find(a1<=0.5))=0;
a1(find(a1>0.5))=1;
a2 = rand(1,200000);
a2(find(a2<=0.5))=0;
a2(find(a2>0.5))=1;

% Assign direction of magnetization
j=+1;
k=+1;
for i = 1:length(a1)
    if(a1(i)~=0)
        a1(i)=a1(i)*j;
        j = j*-1;
    end
    if(a2(i)~=0)
        a2(i)=a2(i)*k;
        k = k*-1;
    end    
end

%%%%%%%%%%%%%% CHANNEL: Generate s1, s2 %%%%%%%%%%%%%

% Generate discrete-time channel coefficients
PW1 = ND1 * T;          % PW50 of the channel with lower normalized density
PW2 = ND2 * T;          % PW50 of the channel with higher normalized density
delta = 0.1;
t=[-15*T:delta:15*T];
h1 = (2*sqrt(2/pi))./(1 + (2*t/(ND1*T)).^2);           % Channel 1
if(ChannelIndex==0)
    h2 = (2*sqrt(2/pi))*(ND1/ND2)./(1 + (2*t/(ND2*T)).^2); % Channel 2
else
    h2 = (2*sqrt(2/pi))./(1 + (2*t/(ND2*T)).^2); % Channel 2
end
x11=0; x12=0; x21=0; x22=0;
% Calculate autocorrelation of the channel response (Channel Filter +
% Matched filter)
if(ChannelIndex==0)
	for i = -floor(FIRChannelLength/2):1:floor(FIRChannelLength/2)
        p1 = (2*sqrt(2/pi))./(1 + (2*(t+i*T)/(ND1*T)).^2);
        p2 = (2*sqrt(2/pi))*(ND1/ND2)./(1 + (2*(t+i*T)/(ND2*T)).^2);
        x11(end+1) = trapz(h1.*p1)*delta;
        x12(end+1) = trapz(h1.*p2)*delta;
        x21(end+1) = trapz(h2.*p1)*delta;
        x22(end+1) = trapz(h2.*p2)*delta;    
	end
else
	for i = -floor(FIRChannelLength/2):1:floor(FIRChannelLength/2)
        p1 = (2*sqrt(2/pi))./(1 + (2*(t+i*T)/(ND1*T)).^2);
        p2 = (2*sqrt(2/pi))./(1 + (2*(t+i*T)/(ND2*T)).^2);
        x11(end+1) = trapz(h1.*p1)*delta;
        x12(end+1) = trapz(h1.*p2)*delta;
        x21(end+1) = trapz(h2.*p1)*delta;
        x22(end+1) = trapz(h2.*p2)*delta;
	end    
end
x11 = x11(2:end); x12 = x12(2:end); x21 = x21(2:end); x22 = x22(2:end); % Removing leading zero
%a1_os = zeros(1,10*length(a1)+10);
%a2_os = zeros(1,10*length(a2)+10);
%for i=11:10:length(a1_os)
%    a1_os(i)=a1((i-1)/10);
%    a2_os(i)=a2((i-1)/10);
%end
%s1 = filter(x11,1,a1_os) + filter(x21,1,a2_os);
%s2 = filter(x22,1,a2_os) + filter(x12,1,a1_os);
%s1 = filter(x11,1,a1) + filter(x21,1,a2);
%s2 = filter(x22,1,a2) + filter(x12,1,a1);
s1 = conv(x11,a1) + conv(x21,a2);
s2 = conv(x22,a2) + conv(x12,a1);
s1 = s1(ceil(FIRChannelLength/2):end-floor(FIRChannelLength/2));
s2 = s2(ceil(FIRChannelLength/2):end-floor(FIRChannelLength/2));

%%%%%%%%%%%%%% Calculate R_a1, R_a2, R_a1a2, R_s1, R_s2, R_s1s2, Rs1a1, Rs1a2, Rs2a1, Rs2a2 %%%%%%%%%%%%%
fprintf(1,'Calculating various auto-correlation matrices...\n');
% Autocorrelation of input a1
Ra1 = zeros(L,L);
temp = xcorr(a1,a1);
Ra1(1,:)=temp(length(a1):length(a1)+L-1);
for i=2:L
    Ra1(i,:)=Ra1(1,abs([-(i-1):1:L-i])+1);
end
% Autocorrelation of input a2
Ra2 = zeros(L,L);
temp = xcorr(a2,a2);
Ra2(1,:)=temp(length(a2):length(a2)+L-1);
for i=2:L
    Ra2(i,:)=Ra2(1,abs([-(i-1):1:L-i])+1);
end
% Autocorrelation of Output s1
Rs1 = zeros(2*N+1,2*N+1);
temp = xcorr(s1,s1);
Rs1(1,:)=temp(length(s1):length(s1)+2*N);
for i=2:2*N+1
    Rs1(i,:)=Rs1(1,abs([-(i-1):1:(2*N+1)-i])+1);
end
% Autocorrelation of Output s2
Rs2 = zeros(2*N+1,2*N+1);
temp = xcorr(s2,s2);
Rs2(1,:)=temp(length(s2):length(s2)+2*N);
for i=2:2*N+1
    Rs2(i,:)=Rs2(1,abs([-(i-1):1:(2*N+1)-i])+1);
end
fprintf(1,'Calculating various cross-correlation matrices...\n');
% Cross-Correlation between inputs a1 and a2
Ra1a2 = zeros(L,L);
temp = xcorr(a1,a2);    % make sure a1 and a2 are of the same length
if(length(a1)~=length(a2))
    fprintf(1,'ERROR: Length of inputs a1 and a2 are not same\n');
    return;
end
Ra1a2(1,:)=temp(length(a1):length(a1)+L-1);
for i=2:L
    Ra1a2(i,:) = temp(length(a1)-(i-1):length(a1)+L-1-(i-1));
end
Ra2a1 = Ra1a2';
% Cross-Correlation between outputs s1 and s2
Rs1s2 = zeros(2*N+1,2*N+1);
temp = xcorr(s1,s2);    % make sure a1 and a2 are of the same length
if(length(s1)~=length(s2))
    fprintf(1,'ERROR: Length of outputs s1 and s2 are not same\n');
    return;
end
for i=1:2*N+1
    Rs1s2(i,:) = temp(length(s1)-(i-1):length(s1)+2*N-(i-1));
end
Rs2s1 = Rs1s2';
% Cross-correlation between output s1 and input a1
% Rs1a1 = [ R(N) R(N+1) ... R(N+L-1) ;
%           R(N-1) ........ R(N+L-2) ;
%           ....
%           R(-N) ......... R(-N+L-1)]
% Where, E[s1(k-i)*a1(k-j)] = R(j-i), -N <= i <= N  and 0 <= j <= L-1
Rs1a1 = zeros(2*N+1,L);
temp = xcorr(s1,a1);
if(length(s1)~=length(a1))
    fprintf(1,'ERROR: Length of output s1 and input a1 are not same\n');
    return;
end
for i=1:2*N+1
    Rs1a1(i,:)=temp(length(s1)+N-(i-1):length(s1)+N+L-1-(i-1));
end
% Cross-correlation between output s1 and input a2
Rs1a2 = zeros(2*N+1,L);
temp = xcorr(s1,a2);
if(length(s1)~=length(a2))
    fprintf(1,'ERROR: Length of output s1 and input a2 are not same\n');
    return;
end
for i=1:2*N+1
    Rs1a2(i,:)=temp(length(s1)+N-(i-1):length(s1)+N+L-1-(i-1));
end
% Cross-correlation between output s2 and input a1
Rs2a1 = zeros(2*N+1,L);
temp = xcorr(s2,a1);
if(length(s2)~=length(a1))
    fprintf(1,'ERROR: Length of output s2 and input a1 are not same\n');
    return;
end
for i=1:2*N+1
    Rs2a1(i,:)=temp(length(s2)+N-(i-1):length(s2)+N+L-1-(i-1));
end
% Cross-correlation between output s2 and input a2
Rs2a2 = zeros(2*N+1,L);
temp = xcorr(s2,a2);
if(length(s2)~=length(a2))
    fprintf(1,'ERROR: Length of output s2 and input a1 are not same\n');
    return;
end
for i=1:2*N+1
    Rs2a2(i,:)=temp(length(s2)+N-(i-1):length(s2)+N+L-1-(i-1));
end

%%%%%%%%%%%%%% Calculate block matrices R_A, R_S, R_SA %%%%%%%%%%%%%

fprintf(1,'Calculating block matrices RA, RS, RSA....\n');
RA = [Ra1 Ra1a2; Ra2a1 Ra2];
RS = [Rs1 Rs1s2; Rs2s1 Rs2];
RSA = [Rs1a1 Rs1a2; Rs2a1 Rs2a2];
RSA_transpose = [Rs1a1' Rs2a1'; Rs1a2' Rs2a2'];
I = [1 zeros(1,L-1)]';
% Calculate Inverse of RS
Rs1_inv = inv(Rs1);
W = Rs2 - Rs2s1*Rs1_inv*Rs1s2;
W_inv = inv(W);
RS_inv_11 = Rs1_inv+Rs1_inv*Rs1s2*W_inv*Rs2s1*Rs1_inv;
RS_inv_12 = -Rs1_inv*Rs1s2*W_inv ;
RS_inv_21 = -W_inv*Rs2s1*Rs1_inv;
RS_inv_22 = W_inv;
RS_inv = [RS_inv_11 RS_inv_12 ; RS_inv_21 RS_inv_22];
clear W W_inv;

%%%%%%%%%%%%%% Calculate Langrange multiplier %%%%%%%%%%%%%

fprintf(1,'Calculating Lagrange multipliers....\n');
% Calculate M = RSA' * RS_inv * RSA - RA
% RSA' * RS_inv
temp1_11 = [Rs1a1'*RS_inv_11 + Rs2a1'*RS_inv_21];
temp1_12 = [Rs1a1'*RS_inv_12 + Rs2a1'*RS_inv_22];
temp1_21 = [Rs1a2'*RS_inv_11 + Rs2a2'*RS_inv_21];
temp1_22 = [Rs1a2'*RS_inv_12 + Rs2a2'*RS_inv_22];
% RSA' * RS_inv * RSA
temp2_11 = [temp1_11*Rs1a1 + temp1_12*Rs2a1];
temp2_12 = [temp1_11*Rs1a2 + temp1_12*Rs2a2];
temp2_21 = [temp1_21*Rs1a1 + temp1_22*Rs2a1];
temp2_22 = [temp1_21*Rs1a2 + temp1_22*Rs2a2];
clear temp1;
% M1 = RA - RSA' * RS_inv * RSA
M_11 = Ra1 - temp2_11;
M_12 = Ra1a2 - temp2_12;
M_21 = Ra2a1 - temp2_21;
M_22 = Ra2 - temp2_22;
M = [M_11 M_12 ; M_21 M_22];
clear temp2;
% Calculate M_inv = inverse of block matrix M
M_11_inv = inv(M_11);
W = M_22 - M_21*M_11_inv*M_12;
W_inv = inv(W);
M_inv_11 = M_11_inv + M_11_inv*M_12*W_inv*M_21*M_11_inv;
M_inv_12 = -M_11_inv*M_12*W_inv;
M_inv_21 = -W_inv*M_21*M_11_inv;
M_inv_22 = W_inv;
M_inv = [M_inv_11 M_inv_12 ; M_inv_21 M_inv_22];

clear M_11 M_12 M_21 M_22;

%%%%%%%%%%%%%%%%% Calculate Lambda *********************

d = [M_inv_11(1,1) M_inv_12(1,1) ; M_inv_21(1,1) M_inv_22(1,1)];
Lambda = sqrt(1/det(d));

%%%%%%%%%%%%%% Calculate F %%%%%%%%%%%%%

fprintf(1,'Calculating GPR Targets....\n');
% Calculate first coefficient of F %
F_first_coeffs = chol(Lambda*d);    % Cholesky factorization: Lambda * d = F_first_coeffs' * F_first_coeffs;
F_first_coeffs = F_first_coeffs';
% Calculate other coefficients of F %
F = Lambda * M_inv * [F_first_coeffs(2,2) -F_first_coeffs(2,1) ; zeros(L-1,2) ; -F_first_coeffs(1,2) F_first_coeffs(1,1) ; zeros(L-1,2)];

%%%%%%%%%%%%%% Calculate C %%%%%%%%%%%%%

fprintf(1,'Calculating Equalizer filter coefficients....\n');
C = RS_inv * RSA * F;

% c11 = (RS_inv_11*Rs1a1 + RS_inv_12*Rs2a1)*f11 + (RS_inv_11*Rs1a2 + RS_inv_12*Rs2a2)*f21;
% c21 = (RS_inv_21*Rs1a1 + RS_inv_22*Rs2a1)*f11 + (RS_inv_21*Rs1a2 + RS_inv_22*Rs2a2)*f21;
% c12 = (RS_inv_11*Rs1a1 + RS_inv_12*Rs2a1)*f12 + (RS_inv_11*Rs1a2 + RS_inv_12*Rs2a2)*f22;
% c22 = (RS_inv_21*Rs1a1 + RS_inv_22*Rs2a1)*f12 + (RS_inv_21*Rs1a2 + RS_inv_22*Rs2a2)*f22;
% C1 = [c11 ; c21];
% C2 = [c12 ; c22];
% C=[C1 C2];
return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%% Map Binary Sequence to Ternary Sequences %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [a] = MapBinaryToTernary(BinarySeq)
% 3 binary bits mapped to 2 ternary bits
% Gray code mapping
t=[ 0 1;
    0 2;
    2 2;
    1 2;
    2 0;
    1 0;
    2 1;
    1 1];
clear a;
a=0;
BinarySeq(end:end+mod(length(BinarySeq),3)-1)=0;
for i = 1:3:length(BinarySeq)
    index = sprintf('%s%s%s',num2str(BinarySeq(i)),num2str(BinarySeq(i+1)),num2str(BinarySeq(i+2)));
    index = bin2dec(index) + 1;
    a(end+1:end+2) = t(index,:);
end
a = a(2:end); % remove the leading zero
return;
