randso3 := proc()
uses Statistics;
    X,V,A,B,R := randso30();
    Sample(X,V);
    r := sqrt(add(V[i]^2,i=1..3));
    v := [seq(V[i]/r,i=1..3)];
    r := v[1]^2+v[2]^2;
    t := randf(0,2*Pi);
    B[1,1],B[1,2],B[1,3] := (v[1]^2*v[3]+v[2]^2)/r,v[1]*v[2]*(v[3]-1)/r,v[1];
    B[2,1],B[2,2],B[2,3] := v[1]*v[2]*(v[3]-1)/r,(v[2]^2*v[3]+v[1]^2)/r,v[2];
    B[3,1],B[3,2],B[3,3] := -v[1],-v[2],v[3];
    R[1,1],R[1,2],R[1,3] := cos(t),-sin(t),0;
    R[2,1],R[2,2],R[2,3] := sin(t),cos(t),0;
    R[3,1],R[3,2],R[3,3] := 0,0,1;
    for i from 1 to 3 do
        for j from 1 to 3 do
            A[i,j] := add(B[i,k]*R[k,j],k=1..3);
        end do;
    end do;
    for i from 1 to 3 do
        for j from 1 to 3 do
            R[i,j] := add(A[i,k]*B[j,k],k=1..3);
        end do;
    end do;
    return R;
end proc;

#allocate a matrix
randso30 := proc()
option remember;
uses Statistics;
    return RandomVariable(Normal(0,1)),allocla[float[8]](3,[3,3],[3,3],[3,3]);
end proc;

#sample N points from randim
sampim := proc(N,so3map)
    m := 3*28*28;
    A,V := allocla[float[8]]([N,m],m);
    for k from 1 to N do
        R := randso3();
        so3map(R,V);
        setrow1(A,k,V,m);
    end do;
    return A;
end proc;

#return true if V is not within r of any vector in L for max of min
#landmark selection
landim0 := proc(L::Array(datatype=float[8]),V::Array(datatype=float[8]),r::float[8],n::integer[4],m::integer[4])
    r2 := r*r;
    for i from 1 to n do
        a := 0.0;
        k := 0;
        for j from 1 to m do
            c := L[i,j]-V[j];
            a := a+c*c;
            k := k+1;
            if(k=100) then
                if(a>r2) then
                    break;
                end if;
                k := 0;
            end if;
        end do;
        if(a<=r2) then
            return false;
        end if;
    end do;
    return true;
end proc;

landim0 := Compiler:-Compile(landim0);

#max of min landmark set from a function rotvec:SO(3)->R^(3*28*28)
landim := proc(N,r,so3map)
    m := 3*28*28;
    V := allocla[float[8]](m);
    T := vectab(m);
    n := 0;
    for k from 1 to N do
        tprint[10]("%d landmarks,%d samples...",n,k);
        R := randso3();
        so3map(R,V);
        if(landim0(T:-A,V,r,n,m)) then
            n := n+1;
            T:-addelt(V);
        end if;
    end do;
    return T:-A[1..n,1..m];
end proc;

so3mapa0 := proc(R::Array(datatype=float[8]),V::Array(datatype=float[8]))
    k := 0;
    for c from 1 to 3 do
        for i from 1 to 28 do
            p1 := -1+(i-.5)/28*2;
            for j from 1 to 28 do
                k := k+1;
                p2 := -1+(j-.5)/28*2;
                r := sqrt(p1*p1+p2*p2);
                if(r>=.99) then
                    V[k] := 1.0;
                else
                    V[k] := R[c,1]*p1+R[c,2]*p2+R[c,3]*sqrt(1-r*r);
                end if;
            end do;
        end do;
    end do;
end proc;

so3mapa0 := Compiler:-Compile(so3mapa0);

#map which rotates an image corresponding each RGB color to it's x,y,z
#coordinates on the sphere
so3mapa := proc(R,V)
    so3mapa0(R,V);
    return;
end proc;

sampima := proc(N)
    sampim(N,so3mapa);
end proc;

landima := proc(N,r)
    return landim(N,r,so3mapa);
end proc;

so3mapb0 := proc(R::Array(datatype=float[8]),V::Array(datatype=float[8]))
    k := 0;
    for c from 1 to 3 do
        for i from 1 to 28 do
            p1 := -1+(i-.5)/28*2;
            for j from 1 to 28 do
                k := k+1;
                p2 := -1+(j-.5)/28*2;
                r := sqrt(p1*p1+p2*p2);
                if(r>=.99) then
                    V[k] := 1.0;
                else
                    p3 := sqrt(1-r*r);
                    q1 := R[1,1]*p1+R[1,2]*p2+R[1,3]*p3;
                    q2 := R[2,1]*p1+R[2,2]*p2+R[2,3]*p3;
                    q3 := R[3,1]*p1+R[3,2]*p2+R[3,3]*p3;
                    if(c=3) then
                        V[k] := 0.0;
                    elif(c=1) then
                        if(q3>0) then
                            V[k] := 0.0;
                        else
                            t := evalf(arccos(q1));
                            if(t>evalf(Pi/2)) then
                                V[k] := sin(2*t-Pi)*(-q3);
                            else
                                V[k] := 0.0;
                            end if;
                        end if;
                    elif(c=2) then
                        if(q3>0) then
                            V[k] := 0.0;
                        else
                            t := evalf(arccos(q1));
                            if(t>evalf(Pi/2)) then
                                V[k] := 0.0;
                            else
                                V[k] := sin(2*t)*(-q3);
                            end if;
                        end if;
                    end if;
                end if;
            end do;
        end do;
    end do;
end proc;

so3mapb0 := Compiler:-Compile(so3mapb0);

#interesting pattern with two red and green regions on one side
so3mapb := proc(R,V)
    so3mapb0(R,V);
    return;
end proc;

sampimb := proc(N)
    sampim(N,so3mapb);
end proc;

landimb := proc(N,r)
    return landim(N,r,so3mapb);
end proc;

so3mapc0 := proc(R::Array(datatype=float[8]),V::Array(datatype=float[8]))
    k := 0;
    for c from 1 to 3 do
        for i from 1 to 28 do
            p1 := -1+(i-.5)/28*2;
            for j from 1 to 28 do
                k := k+1;
                p2 := -1+(j-.5)/28*2;
                r := sqrt(p1*p1+p2*p2);
                if(r>=.99) then
                    V[k] := 1.0;
                    next;
                end if;
                p3 := sqrt(1-r*r);
                q1 := R[1,1]*p1+R[1,2]*p2+R[1,3]*p3;
                q2 := R[2,1]*p1+R[2,2]*p2+R[2,3]*p3;
                q3 := R[3,1]*p1+R[3,2]*p2+R[3,3]*p3;
                if(q3<0) then
                    V[k] := 0.0;
                    next;
                end if;
                if(c=1) then
                    V[k] := q1*q3;
                elif(c=2) then
                    V[k] := q2*q3;
                else
                    V[k] := q3*q3;
                end if;
            end do;
        end do;
    end do;
end proc;

so3mapc0 := Compiler:-Compile(so3mapc0);

so3mapc := proc(R,V)
    so3mapc0(R,V);
    return;
end proc;

sampimc := proc(N)
    sampim(N,so3mapc);
end proc;

landimc := proc(N,r)
    return landim(N,r,so3mapc);
end proc;

#draw a pictures of the vector
drawso3 := proc(V)
    im := getim([28,28],[-1..1,-1..1]);
    ans := allocarr[float[8]]([28,28,3]);
    for i from 1 to 3 do
        im:-setvec(V[(i-1)*784+1..i*784]);
        ans[..,..,i] := im:-arr;
    end do;
    if(type(procname,indexed) and op(procname)='HSV') then
        for i from 1 to 28 do
            for j from 1 to 28 do
                x,y,z := ans[i,j,1],ans[i,j,2],ans[i,j,3];
                r := sqrt(x^2+y^2);
                if(r>1) then
                    ans[i,j,1],ans[i,j,2],ans[i,j,3] := 0.0,0.0,1.0;
                    next;
                elif(r<.0000001) then
                    ans[i,j,1],ans[i,j,2],ans[i,j,3] := 0.0,1.0,0.0;
                    next;
                end if;
                h := arccos(x/r);
                if(y<0) then
                    h := evalf(2*Pi-h);
                end if;
                h := evalf(h*360/2/Pi);
                s := r;
                v := z;
                ans[i,j,1],ans[i,j,2],ans[i,j,3] := h,s,v;
            end do;
        end do;
        ans := ImageTools:-HSVtoRGB(ans);
    end if;
    ans1 := allocarr[float[8]]([280,280,3]);
    for c from 1 to 3 do
        for i0 from 1 to 28 do
            for j0 from 1 to 28 do
                for i1 from 1 to 10 do
                    for j1 from 1 to 10 do
                        i := (i0-1)*10+i1;
                        j := (j0-1)*10+j1;
                        ans1[i,j,c] := ans[i0,j0,c];
                    end do;
                end do;
            end do;
        end do;
    end do;
    img := Create(ans1);
    Embed(img);
    return ans;
end proc;

