%Check explicit expression for LO-RHS invariant
%of 2-bit circuit with 4 dressed cnots, 
%assuming b1 perp span(a1, c1)
%and {c0,b0,a0} is a dependent set

clear;

global_defs;

a0= get_unit_vec(randn(3,1));
b0= get_unit_vec(randn(3,1));
c0= get_unit_vec(randn()*a0 +randn()*b0);	
d0= get_unit_vec(randn(3,1));
a1= get_unit_vec(randn(3,1));
b1= get_unit_vec(cross(a1,randn(3,1)));
c1= get_unit_vec(cross(b1,randn(3,1)));
d1= get_unit_vec(randn(3,1));

if(-(cross(a0,b0)'*cross(b0,c0))*(b0'*c0)<0)
	a0=-a0;
endif


cross_ab1= cross(a1,b1);
cross_bc1 = cross(b1,c1);
%cross_abc1= cross(cross_ab1,c1);
%cross_abcc1 = cross(cross_abc1,c1);
%cross_abb1 = cross(cross_ab1,b1);
%cross_abbc1 = cross(cross_abb1,c1);
%cross_abbcc1 = cross(cross_abbc1,c1);
%cross_bcc1 = cross(cross_bc1,c1);

cross_ab0= cross(a0,b0);
cross_bc0 = cross(b0,c0);
%cross_abc0= cross(cross_ab0,c0);
%cross_abcc0 = cross(cross_abc0,c0);
cross_abb0 = cross(cross_ab0,b0);
cross_abbc0 = cross(cross_abb0,c0);
%cross_abbcc0 = cross(cross_abbc0,c0);
cross_bcc0 = cross(cross_bc0,c0);

%vol_0 = cross_ab0'*c0;
vol_1 = cross_ab1'*c1;

ac1 = a1'*c1;
ab0 = a0'*b0;
bc0 = b0'*c0;

sin_bet2 = norm(cross_bc0);
eta = norm(cross_abbc0);
xxo = ac1*(cross_abb0'*c0);
yyo = ab0*bc0*vol_1;

mu = [-ac1*eta, -ab0*ac1*sin_bet2, (cross_abb0'*c0)*vol_1];

nu = [ab0*vol_1*sin_bet2, vol_1*eta, ab0*bc0*ac1];

gx0= c0;
gy0= cross_bc0/sin_bet2;
gz0= -cross_bcc0/sin_bet2;

gx1= c1;
gy1= cross_bc1;
gz1= b1;

v1 = gy0;
v2 = gz0;
v3 = gx0;

u1 = gy0;
u2 = gz0;
u3 = gx0;

inv1 = dr11(c1,c0)*dr11(b1,b0)*dr11(a1,a0)*dr11(-a1,-a0)*dr11(-b1,-b0)*dr11(-c1,-c0);
inv2 = (-xxo -i*yyo)*eye(4) + nu(1)*kron(sig(gx1),sig(v1)) + nu(2)*kron(sig(gy1),sig(v2)) + nu(3)*kron(sig(gz1),sig(v3)) +i*(mu(1)*kron(sig(gx1),sig(u1)) + mu(2)*kron(sig(gy1),sig(u2))+ mu(3)*kron(sig(gz1),sig(u3)));
error_3cnots = norm(inv1 - inv2)

Mmu = mu(1)*gx1*gy0' +mu(2)*gy1*gz0' +mu(3)*gz1*gx0';
Mnu = nu(1)*gx1*gy0' +nu(2)*gy1*gz0' +nu(3)*gz1*gx0';
Mmu_pauli = mu(1)*kron(sig(gx1),sig(u1)) +mu(2)*kron(sig(gy1),sig(u2)) +mu(3)*kron(sig(gz1),sig(u3));
Mnu_pauli = nu(1)*kron(sig(gx1),sig(v1)) +nu(2)*kron(sig(gy1),sig(v2)) +nu(3)*kron(sig(gz1),sig(v3));
Mmu_d0 = Mmu*d0;
Mnu_d0 = Mnu*d0;
Mmu_t_d1 = Mmu'*d1;
Mnu_t_d1 = Mnu'*d1;
x0 = cross(Mmu_t_d1,d0);
y0 = cross(Mnu_t_d1,d0);
x1 = cross(Mmu_d0,d1);
y1 = cross(Mnu_d0,d1);

lam4r = -d1'*Mnu_d0;
lam4i = -d1'*Mmu_d0;
delx = kron(sig(d1),sig(d0))*(d1'*Mnu_d0)-kron(sig(Mnu_d0),sig(d0))-kron(sig(d1),sig(Mnu_t_d1)) + Mnu_pauli;
dely = kron(sig(d1),sig(d0))*(d1'*Mmu_d0)-kron(sig(Mmu_d0),sig(d0))-kron(sig(d1),sig(Mmu_t_d1)) + Mmu_pauli;

L4r = xxo*kron(sig(d1),sig(d0)) + kron(sig(x1),sig(d0)) + kron(sig(d1),sig(x0)) + delx;
L4i = yyo*kron(sig(d1),sig(d0)) - kron(sig(y1),sig(d0)) - kron(sig(d1),sig(y0)) + dely;

inv1 = dr11(d1,d0)*dr11(c1,c0)*dr11(b1,b0)*dr11(a1,a0)*dr11(-a1,-a0)*dr11(-b1,-b0)*dr11(-c1,-c0)*dr11(-d1,-d0);
inv2 = (lam4r + i*lam4i)*eye(4) + L4r + i*L4i;
error_4cnots = norm(inv1 - inv2)