python_test

#!/usr/bin/env python
#
# Useful cosines / min q dists:
# 0.04     : 0.99    => 8.1   degrees
# 0.02     : 0.995   => 5.76  degrees
# 0.004    : 0.999   => 2.56  degrees
# 0.002    : 0.9995  => 1.8   degrees
# 0.0004   : 0.9999  => 0.81  degrees
# 0.0002   : 0.99995 => 0.576 degrees
# 0.00004  : 0.99999 => 0.256 degrees
# 0.00002  : 0.999995 => 0.18 degrees
# 0.000004 : 0.999999 => 0.081 degrees
# Note FOV for 35mm is about 45 degrees
# So 5k pixels in 45 degress is about 0.01 degrees per pixel
# 
min_cos_seps = {
    "1.8deg"   :0.9995,
    "200pix35" :0.9995,
    "80pix35"  :0.9999,
    "0.18deg"  :0.999995,
    "20pix35"  :0.999995,
    "0.081deg" :0.999999,
    "8pix35"   :0.999999,
}
min_q_dists = {
    "5.76deg"  :0.02,
    "1.8deg"   :0.002,
    "200pix35" :0.002,
    "80pix35"  :0.0004,
    "0.18deg"  :0.00002,
    "20pix35"  :0.00002,
    "0.081deg" :0.000004,
    "8pix35"   :0.000004,
}

#a Imports
from OpenGL.GLUT import *
from OpenGL.GL import *
import gjslib_c
import math
img_png_n=0
vector_z = gjslib_c.vector(vector=(0,0,1))
vector_x = gjslib_c.vector(vector=(1,0,0))
from filters import *

#a Support functions
#f init_opengl
def init_opengl():
    glutInit(sys.argv)
    glutInitDisplayMode(GLUT_3_2_CORE_PROFILE |GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH)
    glutInitWindowSize(64,64)
    glutCreateWindow("self.window_title")

#f save_as_png
def save_as_png(texture, filename):
        b = gjslib_c.filter(filter="save:%s(0)"%filename)
        b.textures([texture])
        b.compile()
        b.execute()
        pass

#f alu_test
def alu_test(ops):
    for (ts, op, save_filename) in ops:
            b = gjslib_c.filter(filter="glsl:alu_buffers(0,0,0)")
            b.define("OP",op)
            b.compile()
            b.textures( [tb[i] for i in ts] )
            b.execute()
            save_as_png(tb[ts[-1]],save_filename)
            pass
    pass

#f v_of_q
def v_of_q(q):
    q = q.copy().normalize()
    return q.rotate_vector(vector_z)

#f quaternion_average
def quaternion_average(qs):
    vf = gjslib_c.vector(length=3)
    vu = gjslib_c.vector(length=3)
    for q in qs:
        vf += q.rotate_vector(vector_z)
        vu += q.rotate_vector(vector_x)
        pass
    return gjslib_c.quaternion().lookat(vf,vu)

#a Toplevel 1
print "init opengl"        
init_opengl()
# a = gjslib_c.texture(filename='images/IMG_2162.JPG')

print "Create textures"
tb = []
size = 1024
for i in range(12):
        tb.append(gjslib_c.texture(width=size, height=size))
        pass
print tb[0].width, tb[0].height


# b = gjslib_c.filter(filter="glsl:yuv_from_rgb(1,3)&-DINTENSITY_YSCALE=(3456.0/5184.0)&-DINTENSITY_XOFS=0.0&-DINTENSITY_XSCALE=1.0&-DINTENSITY_YOFS=0.0")
# b.define("INTENSITY_YSCALE","3.0")
# b.define("INTENSITY_YSCALE",remove=1)
# b.compile()

#a Match classes
#c c_image_match
class c_image_match(object):
    radius=4
    windowed_equalization = False
    max_corners=3
    max_matches_per_corner=3
    min_corner_distance = 5.0
    min_corner_distance = 60.0
    min_match_distance = 2.5
    def __init__(self, radius=4):
        # radius must be 4 at the moment as circle_dft_diff_combine_filter is fixed    
        self.copy_img = c_alu_filter(extra_defines={"OP":"src_a"})
        self.equalize = c_windowed_equalization_filter()
        self.harris = c_harris_filter()
        self.find_corners = c_find_filter(extra_parameters={"min_distance":self.min_corner_distance, "minimum":0.05, "max_elements":2500})
        self.find_matches = c_find_filter(extra_parameters={"min_distance":self.min_match_distance, "minimum":0.04,  "max_elements":2500})
        self.circle_dft = c_circle_dft_filter(extra_defines={"DFT_CIRCLE_RADIUS":self.radius,
                                                             "CIRCLE_COMPONENT":"r",
                                                             })
        self.circle_dft_diff         = c_circle_dft_diff_filter()
        self.circle_dft_diff_combine = c_circle_dft_diff_combine_filter()
        pass
    def get_matches(self, tb):
        """tb must be at least 10 textures, and the first is the source image, second is the target image"""
        self.harris.execute( (tb[0],tb[4]) )

        if self.windowed_equalization: # do windowed equalization to remove brightness and contrastiness dependence - with loss of information
                self.copy_img.execute((tb[0],tb[0],tb[2]))
                self.equalize.execute((tb[2],tb[0]))
                self.copy_img.execute((tb[1],tb[1],tb[2]))
                self.equalize.execute((tb[2],tb[1]))
                pass
        self.circle_dft.execute((tb[0],tb[2]))
        self.circle_dft.execute((tb[1],tb[3]))
        self.find_corners.execute( (tb[4],) )

        print "Found %d corners (will restrict to max %d)"%(self.find_corners.f.num_points, self.max_corners)
        corners = self.find_corners.f.points[:self.max_corners]
        matches = {}
        for i in range(len(corners)):
            pt = corners[i]
            xy = (pt[0],pt[1])
            for i, dxy in [(0,(1,0)), (1,(0,1)), (2,(-1,0)), (3,(0,-1))]:
                self.circle_dft_diff.set_parameters( {"uv_base_x":xy[0]+dxy[0]*self.radius,
                                                      "uv_base_y":xy[1]+dxy[1]*self.radius} )
                self.circle_dft_diff.execute( (tb[2], tb[3], tb[5+i]) )
                pass
            self.circle_dft_diff_combine.execute( (tb[5], tb[6], tb[7], tb[8], tb[9]) )
            self.find_matches.execute( (tb[9],) )
            matches[xy] = self.find_matches.f.points[0:self.max_matches_per_corner]
            pass
        return matches
    def times(self):
        return {"copy_img":self.copy_img.times(),
                "equalize":self.equalize.times(),
                "harris":self.harris.times(),
                "circle_dft":self.circle_dft.times(),
                "find_corners":self.find_corners.times(),
                "circle_dft_diff":self.circle_dft_diff.times(),
                "circle_dft_diff_combine":self.circle_dft_diff_combine.times(),
                "find_matches":self.find_matches.times(),
                }
    pass

#c c_quaternion_match
class c_quaternion_match(object):
    """
    A match is a mapping from a pair of source points to a pair of target points

    The match has a status
    """
    #f test_add
    @classmethod
    def test_add(cls, src_qs, tgt_qs, max_angle_diff):
        (src_angle, src_axis) = src_qs[0].angle_axis(src_qs[1], vector_z).to_rotation()
        (tgt_angle, tgt_axis) = tgt_qs[0].angle_axis(tgt_qs[1], vector_z).to_rotation()
        if abs((src_angle-tgt_angle))>=abs(src_angle*max_angle_diff): return None
        if False:
            print "  Src/tgt axis/angle",src_axis,src_angle*180/math.pi,tgt_axis,tgt_angle*180/math.pi
            pass
        #dq = (src_qs[0]/tgt_qs[0]).rotate_vector(vector_z) - (src_qs[1]/tgt_qs[1]).rotate_vector(vector_z)
        #print "Abs:",dq.modulus()," ",180.0/math.pi*abs(src_angle-tgt_angle),"  ",abs(src_angle-tgt_angle)/src_angle,"  ",dq
        #dq = dq.modulus()
        #if abs(dq)>0.2: return None
        return cls(src_qs, tgt_qs, src_angle_axis=(src_axis, src_angle), tgt_angle_axis=(tgt_axis,tgt_angle))
    #f __init__
    def __init__(self, src_qs, tgt_qs, src_angle_axis=None, tgt_angle_axis=None ):
        self.src_qs = src_qs
        self.tgt_qs = tgt_qs
        self.src_angle_axis = src_angle_axis
        self.tgt_angle_axis = tgt_angle_axis
        self.src_from_tgt_orient = gjslib_c.quaternion()
        self.diff_q = gjslib_c.quaternion()
        self.calculate()
        pass
    #f calculate
    def calculate(self):
        """
        There is a rotation (angle, axis) that has src_q0.vector_z/src_q1.vector_z on a great circle,
        and similarly for tgt_q0.vector_z/tgt_q1.vector_z
        (The spherical coordinates of a point corresponding to src_q/tgt_q is q.vector_z in our definition)

        Given this we have two great circles; these are either the same, or they meet at two points.
        If they are the same we have src_axis==tgt_axis
        We can find the transformation that rotates src axis to tgt axis - this we call diff_q
        Hence diff_q . src_axis = tgt axis

        Now, diff_q is a rotation about an axis which goes through the great circles for src and tgt
        each in two places. diff_q has no effect on these positions (da0 and -da0).

        So, we can consider a first transformation that moves src_q0.vector_z to da0; this is moving
        the point around the great circle with the axis src_axis. Next apply the diff_q transformation;
        this has no effect on the point. Finally apply a last transformation that moves da0 to tgt_q0.vector_z, 
        moving it along its great circle hence as a rotation around tgt_axis.
        This combination is a transformation for src_q0.vector_z to tgt_q0.vector_z

        Consider the effect on a point src_q1.vector_z - also on the
        great circle, perhaps 10 degrees further clockwise round. 
        Our first rotation will place the point at da0 + 10 degrees clockwise on the src great circle
        The diff_q transformation moves this to da0 + 10 degrees clockwise on the tgt great circle
        The last rotation moves it to tgt_q1.vector_z - i.e. tgt_q0.vector_z + 10 degrees clockwise
        (assuming the src/tgt points correspond).

        The first rotation is a rotation around src_axis to make src_q0.vector_z move to diff_axis.
        This is simply the transformation to move diff_axis to src_q0.vector_z on a great circle
        The last is is also just the transformation to move diff_axis to tgt_q0.vector_z on a great circle

        Now if src_axis == +-tgt_axis then all that is needed is a rotation around src_axis
        """
        if self.src_angle_axis is None:
            self.src_angle_axis = self.src_qs[0].angle_axis(self.src_qs[1], vector_z).to_rotation()
            pass
        if self.tgt_angle_axis is None:
            self.tgt_angle_axis = self.tgt_qs[0].angle_axis(self.tgt_qs[1], vector_z).to_rotation()
            pass
        (src_axis, src_angle) = self.src_angle_axis
        (tgt_axis, tgt_angle) = self.tgt_angle_axis

        sp0 = self.src_qs[0].rotate_vector(vector_z)
        tp0 = self.tgt_qs[0].rotate_vector(vector_z)
        self.diff_q = src_axis.angle_axis_to_v(tgt_axis)
        if True:
            print sp0, tp0, self.diff_q
            pass

        if self.diff_q.r>0.99999999:
            self.src_from_tgt_orient = tp0.angle_axis_to_v(sp0)
            return

        (diff_angle, diff_axis) = self.diff_q.to_rotation()
        print diff_angle, diff_axis

        src_sp0_orient_to_diff_axis = diff_axis.angle_axis_to_v(sp0)
        dst_tp0_orient_to_diff_axis = tp0.angle_axis_to_v(diff_axis)

        self.src_from_tgt_orient = src_sp0_orient_to_diff_axis * ~self.diff_q * dst_tp0_orient_to_diff_axis
        if True:
            print src_sp0_orient_to_diff_axis
            print dst_tp0_orient_to_diff_axis
            print "These two should be equal... (or near as ...)"
            print (self.src_from_tgt_orient * self.tgt_qs[1]).angle_axis(self.src_qs[1],vector_z)
            print (self.src_from_tgt_orient * self.tgt_qs[0]).angle_axis(self.src_qs[0],vector_z)
        pass
    pass

if False:
    qic=gjslib_c.quaternion_image_correlator()
    src_q0 = gjslib_c.quaternion().from_euler(roll=5.0,yaw=20,pitch=10,degrees=1)
    src_q1 = gjslib_c.quaternion().from_euler(pitch=5.0,yaw=10.0,degrees=1) * src_q0
    tgt_q0 = gjslib_c.quaternion().from_euler(roll=20.0,degrees=1) * src_q0
    tgt_q1 = gjslib_c.quaternion().from_euler(roll=20.2,degrees=1) * src_q1
    qm = c_quaternion_match.test_add(src_qs=(src_q0,src_q1),
                                     tgt_qs=(tgt_q0,tgt_q1),
                                     max_angle_diff=0.02) # good matches for 1900/1901 are all <0.
    qic.add_match(src_q0,tgt_q0,0,0,0)
    qic.add_match(src_q1,tgt_q1,0,0,0)
    qic.create_mappings()
    print qm.src_from_tgt_orient
    die

#c c_quaternion_cluster
class c_quaternion_cluster(object):
    #f __init__
    def __init__(self, results):
        self.results = results
        pass
    #f find_centers
    def find_centers(self, qs, cxy, min_dist, min_min_dist, min_number, scale, max_iter):
        def q_dist_from_v(q,v):
            return c_vector.vector_length(c_vector.vector_diff(v_of_q(q),v))
        if max_iter==0:
            return (qs[:], cxy, min_dist, len(qs), max_iter)
        if min_dist < min_min_dist:
            return (qs[:], cxy, min_dist, len(qs), max_iter)
        max_min = 0
        allowed_qs = []
        
        for q in qs:
            d = q_dist_from_v(q,cxy)
            if (d<min_dist):
                if (d>max_min): max_min=d
                allowed_qs.append(q)
                pass
            pass
        if len(allowed_qs)<min_number:
            return (qs[:], cxy, min_dist, len(allowed_qs), max_iter)
        tq = quaternion_average(allowed_qs)
        return self.find_centers(allowed_qs, v_of_q(tq), max_min*scale, min_min_dist, min_number, scale, max_iter-1)
    #f find_clusters
    def find_clusters(self):
        #b Produce list of results from valid results
        qs = []
        for m in self.results:
            qs.append(self.results[m])
            pass

        #b Find best centers from results
        handled_results = []
        remaining_qs = qs[:]
        fcs = []
        while len(remaining_qs)>0:
            q = remaining_qs[0]
            v = v_of_q(q)
            fc = self.find_centers(remaining_qs, v, 3.0, 0.005, len(qs)/10, 0.9, 40)
            fcs.append(fc)
            for q in fc[0]:
                handled_results.append(q)
                remaining_qs.remove(q)
                pass
            pass

        #b Sort list of centers
        def fc_cmp(x,y):
            if x[2]/len(x[0])<y[2]/len(y[0]): return -1
            return 1
        fcs.sort(cmp=fc_cmp)
        if True:
            for fc in fcs:
                print fc[2], fc[1], fc[3], fc[4]
                n = 0
                for q in fc[0]:
                    print "  ",q
                    n+=1
                    if n>30:
                        print "  ...."
                        break
                    pass
                pass
            pass

        #b Display list of centers
        agreed_orientations = []
        for fc in fcs:
            tq = quaternion_average(fc[0])
            print "distance",fc[2],"num_in_range",fc[3],"/",len(fc[0]),"iterations left",fc[4], "center (r=",tq.r,",i=",tq.i,",j=",tq.j,",k=",tq.k,")"
            agreed_orientations.append( {"quaternion":tq, "error":fc[2]/len(fc[0])} )
            pass
        return agreed_orientations
    pass

#c c_camera_image
class c_camera_image(object):
    def __init__(self, image_filename, frame_width=22.3, focal_length=35.0, lens_type="rectilinear", orientation=None):
        self.texture = gjslib_c.texture(filename=image_filename)
        # The lp has to have width/height such that a uv of 0 to 1 maps from left to right, and bottom to top
        # The uv is derived from the (-1,1) range, i.e. uv can also be conceived as -1=left, -1=bottom, +1=right, +1=top
        # Hence for images that are wider than high, we want -1=left, +1=right; but the same angle rotated by 90 is off the top
        # So the uv for that must be = width/height. Hence, for wider than high, width=2.0, height=2.0*width/height
        # For example, an image that is 300 wide and 200 high will have width=2.0, height=3.0
        self.lp      = gjslib_c.lens_projection(width=2.0,
                                                height=2.0*self.texture.width/self.texture.height,
                                                frame_width=frame_width,
                                                focal_length=focal_length,
                                                lens_type=lens_type)
        if orientation is None:
            orientation = gjslib_c.quaternion(r=1)
            pass
        self.lp.orient(orientation)
        pass
    def orientation(self):
        return self.lp.orientation
    def orientation_of_xy(self,xy):
        return self.lp.orientation_of_xy(xy)
    def orientation_of_xy_list(self, xy_list):
        q_list = []
        for xy in xy_list:
            q_list.append(self.lp.orientation_of_xy(xy))
            pass
        return q_list
    def xy_of_orientation_list(self, q_list):
        xy_list = []
        for q in q_list:
            xy_list.append(self.lp.xy_of_orientation(q))
            pass
        return xy_list

#c c_mapping
class c_mappings(object):
    """
    The set tgt_q_mappings[tgt_q] for a particular src_q is the list
    of other src_q/tgt_q pairs that have approximately the same angular distance
    between srcs as between tgts, stored with the quaternion transformation that would acheive tgt->src
    for these pairs.
    Note that a source can only map to one target point (!) at any one time,
    therefore only one tgt_q_mappings is potentially viable for any one source
    and then only ones that are 'similar' to a particular transformation
    Hence a 'score' can be calculated for each src/tgt pair with a count of the other src/tgt pairs
    that have a similar angular distance src->src, tgt->tgt and which have a similar transformation tgt->src
    """
    #f save_mappings
    @staticmethod
    def save_mappings(mappings_by_src_q, f, src_from_tgt_q=None, best_matches=None, qic=None):
        cbm = c_image_pair_quaternion_match.c_best_match
        def quaternion_as_string(q):
            return "quaternion(r=%f,i=%f,j=%f,k=%f)"%(q.r, q.i, q.j, q.k)

        print >>f, "import gjslib_c"
        print >>f, "quaternion = gjslib_c.quaternion"

        print >>f, "base_mapping = None"
        if src_from_tgt_q:
            print >>f, "base_mapping = %s"%quaternion_as_string(src_from_tgt_q)

        print >>f, "src_quaternions = {"
        if qic:
            for src_q in cbm.qic_qs:
                src_q = cbm.qic_qs[src_q]
                if src_q[2]=="src":
                    print >>f, "    %d:%s,"%(src_q[1], quaternion_as_string(src_q[0]))
                    pass
                pass
            pass
        else:
            for src_q in mappings_by_src_q:
                print >>f, "    %d:%s,"%(src_q.id, quaternion_as_string(src_q))
                pass
            pass
        print >>f, "    }"

        print >>f, "tgt_quaternions = {"
        tgt_qs = []
        if qic:
            for tgt_q in cbm.qic_qs:
                tgt_q = cbm.qic_qs[tgt_q]
                if tgt_q[2]=="tgt":
                    print >>f, "    %d:%s,"%(tgt_q[1], quaternion_as_string(tgt_q[0]))
                    pass
                pass
            pass
        else:
            for src_q in mappings_by_src_q:
                for tgt_q in mappings_by_src_q[src_q].tgt_qs:
                    if tgt_q not in tgt_qs:
                        tgt_qs.append(tgt_q)
                        print >>f, "    %d:%s,"%(tgt_q.id, quaternion_as_string(tgt_q))
                        pass
                    pass
                pass
            pass
        print >>f, "    }"

        print >>f, "best_matches = ["
        if best_matches:
            for bm in best_matches:
                #bm.mappings is list of src/tgt tuples
                print >>f, "    (%f, %f, %s, ["%(bm.max_distance, bm.min_cos_sep, quaternion_as_string(bm.src_from_tgt_q))
                qs = []
                for (src_q, tgt_q, num_uses) in bm.best_mappings:
                    if qic:
                        print >>f, "        ((%d, %d),["%(src_q,tgt_q),
                        pass
                    else:
                        print >>f, "        ((%d, %d),["%(src_q.id,tgt_q.id),
                        pass
                    for src_tgts in bm.qs:
                        (src_q0, tgt_q0, src_q1, tgt_q1) = src_tgts
                        (src_from_tgt_orient, d) = bm.qs[src_tgts]
                        if ((src_q0==src_q and tgt_q0==tgt_q) or
                            (src_q1==src_q and tgt_q1==tgt_q) ):
                            qs.append(src_from_tgt_orient)
                            pass
                        if src_q0==src_q and tgt_q0==tgt_q:
                            if qic:
                                print >>f, "(%d,%d,%f),"%(src_q1, tgt_q1, d),
                                pass
                            else:
                                print >>f, "(%d,%d,%f),"%(src_q1.id, tgt_q1.id, d),
                                pass
                            pass
                        pass
                    print >>f, "] ),"
                    pass
                if len(qs)==0: qs.append(gjslib_c.quaternion(1))
                print >>f, "        ], {"
                for (src_q, tgt_q, num_uses) in bm.best_mappings:
                    if qic:
                        print >>f, "            %d:(%d,),"%(src_q, tgt_q)
                        pass
                    else:
                        print >>f, "            %d:(%d,),"%(src_q.id, tgt_q.id)
                        pass
                    pass
                print >>f, "        }, %s,"%quaternion_as_string(bm.optimized_src_from_tgt_q)#quaternion_average(qs))
                print >>f, "        ),"
            pass
        print >>f, "    ]"

        print >>f, "mappings={}"
        if qic:
            for src_qx in qic.src_qs():
                src_q = cbm.qid_from_q(src_qx)
                print >>f, "mappings[%d] = {"%src_q
                print >>f, "    'tgts':{"
                for tgt_qx in qic.tgt_qs_of_src_q(src_qx):
                    tgt_q = cbm.qid_from_q(tgt_qx)
                    print >>f, "            %d:{"%tgt_q
                    #(tgt_xy, tgt_qx, fft_rot, fft_power) = mapping.matches[tgt_q]
                    #print >>f, "                'data':(%s,%f,%f),"%(str(tgt_xy),fft_rot,fft_power)
                    print >>f, "                'mappings':("
                    src_tgt_mappings = qic.src_tgt_mappings(src_qx, tgt_qx)
                    for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_q) in src_tgt_mappings:
                        print >>f, "                    (%d, %d, %s, %d),"  %(cbm.qid_from_q(src_q1),
                                                                              cbm.qid_from_q(tgt_q1),
                                                                              quaternion_as_string(src_from_tgt_q),
                                                                              0)
                        pass
                    print >>f, "                ),"
                    print >>f, "            },"
                    pass
                print >>f, "        },"
                print >>f, "    }"
                pass
            pass
        else:
            for src_q in mappings_by_src_q:
                mapping = mappings_by_src_q[src_q]
                print >>f, "mappings[%d] = {"%src_q.id
                print >>f, "    'tgts':{"
                for tgt_q in mapping.tgt_qs:
                    print >>f, "            %d:{"%tgt_q.id
                    (tgt_xy, tgt_qx, fft_rot, fft_power) = mapping.matches[tgt_q]
                    print >>f, "                'data':(%s,%f,%f),"%(str(tgt_xy),fft_rot,fft_power)
                    print >>f, "                'mappings':("
                    for (src_q1, tgt_q1, src_from_tgt_orient) in mapping.tgt_q_mappings[tgt_q]:
                        print >>f, "                    (%d, %d, %s, %d),"  %(src_q1.id,tgt_q1.id,quaternion_as_string(src_from_tgt_orient),src_from_tgt_orient.id)
                        pass
                    print >>f, "                ),"
                    print >>f, "            },"
                    pass
                print >>f, "        },"
                print >>f, "    }"
                pass
            pass
        pass
    #f show_mappings
    @staticmethod
    def show_mappings(mappings_by_src_q):
        for src_q in mappings_by_src_q:
            mappings_by_src_q[src_q].show_mapping()
            pass
        pass
    #f show_mapping
    def show_mapping(self, skip_unmapped_tgt_qs=True):
        model_src_from_tgt_q = gjslib_c.quaternion(r=0.997425,i=-0.007999,j=-0.070465,k=0.010689)
        vector_z = gjslib_c.vector(vector=(0,0,1))
        print "%s:%s:%d"%(str(self.src_q),
                          str(self.src_q.rotate_vector(vector_z)),
                          len(self.tgt_qs))
        min_error = self.find_min_error(model_src_from_tgt_q, self.src_q)
        if min_error is None: print "    No min error for ",model_src_from_tgt_q.to_rotation_str(1)
        if min_error is not None:print "    min_e for %s:"%model_src_from_tgt_q.to_rotation_str(1),min_error[0],min_error[1]
        for tgt_qx in self.tgt_qs:
            if skip_unmapped_tgt_qs and (len(self.tgt_q_mappings[tgt_qx])==0): continue
            print "    map v,l:%s:%s:%d"%(str(tgt_qx),
                                          str(tgt_qx.rotate_vector(vector_z)),
                                          len(self.tgt_q_mappings[tgt_qx]))
            for (src_q1, tgt_q1, src_from_tgt_orient) in self.tgt_q_mappings[tgt_qx]:
                print "        ", src_from_tgt_orient, src_from_tgt_orient.to_rotation_str(1), src_q1.rotate_vector(vector_z), tgt_q1.rotate_vector(vector_z)
            if tgt_qx in self.selected_mappings:
                print "        sm:",len(self.selected_mappings[tgt_qx])
                for (src_from_tgt_qx, n, e) in self.selected_mappings[tgt_qx]:
                    print "            sel,n,e:",src_from_tgt_qx.to_rotation_str(1),n,e
                    pass
                pass
            if tgt_qx in self.too_far_from:
                print "        tff:",len(self.too_far_from[tgt_qx])
                for (src_from_tgt_qx, max_maa) in self.too_far_from[tgt_qx]:
                    closeness_to_model = src_from_tgt_qx.angle_axis(model_src_from_tgt_q,vector_z).r
                    if closeness_to_model>0.9999:
                        print "            sft:",src_from_tgt_qx.to_rotation_str(1), max_maa, closeness_to_model
                        pass
                    pass
                pass
        pass
    #f score_mapping
    def score_mapping(self, src_from_tgt_q, min_cos_sep=0.9995, max_q_dist=0.02):
        """
        For a given tgt->source transformation, generate a score
        The score is the highest count of matching source/target pairs which
        have a transform that is similar, from the possible targets for this source

        One can also track the 'sum of vectors' for 'z' and 'x' transformed by the
        similar transforms;
        """
        max_score = None
        verbose = False
        for tgt_qx in self.tgt_qs:
            dst = (src_from_tgt_q*tgt_qx).angle_axis(self.src_q,vector_z).r
            if verbose: print "checking",tgt_qx.id,dst
            if dst<min_cos_sep: continue
            if verbose: print "passed src_q/tgt_qx mapping"
            # Does it matter if the source point does not match up with any other target points?
            if len(self.tgt_q_mappings[tgt_qx])==0: continue
            # The more mappings that match the better - indeed, all of 'min_used' should map
            if len(self.tgt_q_mappings[tgt_qx])<4: continue
            if verbose: print "passed mapping length check"
            score = 0
            for m in self.tgt_q_mappings[tgt_qx]:
                (src_q1, tgt_q1, src_from_tgt_orient) = m
                if verbose: print "checking mapping",src_q1.id, tgt_q1.id, src_from_tgt_orient.id
                # Only need to one of compare src_from_tgt or src_q1
                # if ((src_from_tgt_q*tgt_q1).angle_axis(src_q1,vector_z).r)<min_cos_sep: continue
                dq = src_from_tgt_q.distance_to(src_from_tgt_orient)
                if dq>max_q_dist: continue
                score += 1
                pass
            #print "not sure how some of the matches are getting through - there are perfect matches, and "
            if (max_score is None) or (max_score[0]<score):
                max_score = (score, tgt_qx)
                pass
            pass
        return max_score
    #f find_min_error
    def find_min_error(self, src_from_tgt_q, src_qx):
        min_d = None
        for tgt_qx in self.tgt_qs:
            # Does it matter if the source point does not match up with any other target points?
            if len(self.tgt_q_mappings[tgt_qx])==0: continue
            # The more mappings that match the better - indeed, all of 'min_used' should map
            if len(self.tgt_q_mappings[tgt_qx])<4: continue
            # Does it matter if the source point has no approximately similar target mappings?
            can_map = False
            max_maa = 0
            for m in self.tgt_q_mappings[tgt_qx]:
                (src_q1, tgt_q1, src_from_tgt_orient) = m
                maa = src_from_tgt_q.angle_axis(src_from_tgt_orient,vector_z)
                max_maa = max(max_maa,maa.r)
                pass
            if max_maa>0.999: can_map = True
            if not can_map:
                self.too_far_from[tgt_qx].append((src_from_tgt_q, max_maa))
                continue
            # Use the cos(great circle rotation between tgt mapped back to src and src)
            # Not good to use ~src_qx * src_from_tgt_q*tgt_qx directly
            # Can use (~src_qx * src_from_tgt_q*tgt_qx) applied to vector_z and use 1-z
            # Although that is basically identical (really really close)
            rv0 = (~src_qx*src_from_tgt_q*tgt_qx).rotate_vector(vector_z)
            d = 1-rv0.coords[2]*rv0.coords[2]
            if False:
                rq0 = src_qx.angle_axis(src_from_tgt_q*tgt_qx,vector_z)
                d = (1-rq0.r) # Not much effect if we change this to rq0.r^2
                pass
            if (min_d is None) or (min_d[0]>d):
                min_d = (d, tgt_qx)
                pass
            pass
        return min_d
    #f select_for_mapping
    def select_for_mapping(self, src_from_tgt_q, tgt_qx, i, e):
        self.selected_mappings[tgt_qx].append( (src_from_tgt_q, i, e) )
        pass
    #f __init__
    def __init__(self, src_xy, src_q):
        self.src_xy = src_xy
        self.src_q = src_q
        self.matches = {}
        self.tgt_qs = []
        self.tgt_q_mappings = {}
        self.selected_mappings={}
        self.too_far_from = {}
        pass
    #f add_match
    def add_match(self, tgt_xy, tgt_q, fft_rot, fft_power, min_cos_sep=min_cos_seps["8pix35"]):
        found_tgt_q = None
        for tgt_qx in self.tgt_qs:
            cos_angle = tgt_qx.angle_axis(tgt_q,vector_z).r
            if cos_angle>min_cos_sep:
                found_tgt_q = tgt_qx
                break
            pass
        if found_tgt_q is None:
            self.tgt_qs.append(tgt_q)
            self.tgt_q_mappings[tgt_q] = []
            pass
        else:
            #print "Found tgt q",found_tgt_q,tgt_q,cos_angle
            tgt_q = found_tgt_q
            pass
        self.matches[tgt_q] = (tgt_xy, tgt_q, fft_rot, fft_power)
        pass
    #f power_of_match
    def power_of_match(self, tgt_q):
        if tgt_q not in self.tgt_qs:
            return 0
        return self.matches[tgt_q][3]
    #f add_tgt_mapping
    def add_tgt_mapping(self, mappings_by_src_q, tgt_q, src_q1, tgt_q1, qm, max_rot_diff=60.0):
        if tgt_q not in self.tgt_qs:
            raise Exception, "Unexpected add_tgt_mapping for tgt quaternion that is not part of the source"
        if False:#True:
            (src0_xy, tgt0_xy, fft0_rot, fft0_power) = self.matches[tgt_q]
            (src1_xy, tgt1_xy, fft1_rot, fft1_power) = mappings_by_src_q[src_q1].matches[tgt_q1]
            rot_diff = abs(fft1_rot-fft0_rot)
            if rot_diff>180: rot_diff=360-rot_diff
            if rot_diff>max_rot_diff: return
            if False:
                print src0_xy, tgt0_xy, src1_xy,tgt1_xy, rot_diff
                rq = qm.src_from_tgt_orient
                print fft0_power, fft0_rot, fft1_power, fft1_rot, "(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k)
                pass
            pass
        self.tgt_q_mappings[tgt_q].append( (src_q1, tgt_q1, qm.src_from_tgt_orient) )
        self.selected_mappings[tgt_q] = []
        self.too_far_from[tgt_q] = []
        pass
    pass

#c c_image_pair_quaternion_match
class c_image_pair_quaternion_match(object):
    #f __init__
    def __init__(self, filenames=[]):
        self.camera_images = {}
        for f in filenames:
            self.add_image(f)
            pass
        self.im = c_image_match()
        self.im.max_corners=40
        self.im.max_matches_per_corner=10
        self.im.max_corners=20
        self.im.max_matches_per_corner=10 # Not too many matches, as a good match is a good match
        self.to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                                extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                               "GL_POSITION":"vec4(xsc*x+xc,ysc*y+yc,0,1)",
                                               })
        self.mappings_by_src_q = None
        self.src_qs = []
        self.qic = None
        pass
    #f xy_from_texture
    def xy_from_texture(self,texture,texture_xy):
        return ( (2*(float(texture_xy[0])/texture.width)-1.0),
                 (2*(float(texture_xy[1])/texture.height)-1.0) )
    #f orient
    def orient(self, image, orientation):
        self.camera_images[image].lp.orient(orientation)
        pass
    #f xy_of_orientation
    def xy_of_orientation(self, image, q):
        return self.camera_images[image].lp.xy_of_orientation(q)
    #f add_image
    def add_image(self, image_filename="", **kwargs):
        if image_filename not in self.camera_images:
            self.camera_images[image_filename] = c_camera_image(image_filename=image_filename, **kwargs)
            pass
        pass
    #f find_overflows
    def find_overflows(self, images, cxy0, size):
        ci0 = self.camera_images[images[0]]
        ci1 = self.camera_images[images[1]]
        width = ci0.texture.width
        height = ci0.texture.height
        ci0_xy_list = []
        for i in range(4):
            for j in range(21):
                xy = j/10.0-1.0
                xy = [ (xy,-1.0), (1.0,xy), (-xy,1.0), (1.0,-xy) ][i]
                xy = ((cxy0[0] + xy[0]*size/2)*width/2, (cxy0[1] + xy[1]*size/2)*height/2)
                ci0_xy_list.append(xy)
                pass
            pass
        q_list      = ci0.orientation_of_xy_list(ci0_xy_list)
        ci1_xy_list = ci1.xy_of_orientation_list(q_list)
        off_edge = [0]*4
        in_bounds = True
        for i in range(4):
            for j in range(21):
                err = None
                for xy in (ci0_xy_list[i*21+j], ci1_xy_list[i*21+j]):
                    xy = (xy[0]/(width/2), xy[1]/(height/2))
                    for k in range(2):
                        if (xy[k]<-1.0):err = -1 - xy[k]
                        if (xy[k]>1.0): err = xy[k] - 1
                        pass
                    if err is None: continue
                    err = abs(err)
                    if err > off_edge[i]:
                        off_edge[i] = err
                        in_bounds = False
                        pass
                    pass
                pass
            pass
        if in_bounds: return None
        return off_edge
    #f find_overlap
    def find_overlap(self, images, cxy0, size, scale=0.9, max_iter=40):
        ci0 = self.camera_images[images[0]]
        ci1 = self.camera_images[images[1]]
        for i in range(max_iter):
            overflow = self.find_overflows(images, cxy0, size)
            #print i, cxy0, size, overflow
            if overflow is None: return (cxy0, size)
            if overflow[0]>0 and overflow[2]>0:   size *= scale
            elif overflow[1]>0 and overflow[3]>0: size *= scale
            else:
                if overflow[0]>0: cxy0=(cxy0[0] + size*(1-scale), cxy0[1])
                if overflow[2]>0: cxy0=(cxy0[0] - size*(1-scale), cxy0[1])
                if overflow[1]>0: cxy0=(cxy0[0], cxy0[1] + size*(1-scale))
                if overflow[3]>0: cxy0=(cxy0[0], cxy0[1] - size*(1-scale))
                pass
            pass
        return None
    #f find_overflows_for_projections
    def find_overflows_for_projections(self, images, projections):
        def check_bounds(in_bounds, off_edge, edge, image, xy):
            # An image that is 300 wide and 200 high will have width=2.0, height=3.0 for its scaling
            # This means that the xy positions returned are truly just within the image
            # independent of the aspect ratio
            err = None
            for e in (abs(xy[0])-1, abs(xy[1])-1 ):
                if e<=0: continue
                if e > off_edge[edge]:
                    off_edge[edge] = e
                    in_bounds = False
                    pass
                pass
            return (in_bounds, off_edge)
        ci0 = self.camera_images[images[0]]
        ci1 = self.camera_images[images[1]]
        ci0_tp = projections[0]
        ci1_tp = projections[1]
        q0_list = []
        q1_list = []
        for i in range(4):
            for j in range(21):
                xy = j/10.0-1.0 # in range -1<=xy<1
                xy = [ (xy,-1.0), (1.0,xy), (-xy,1.0), (-1.0,-xy) ][i] # (-1,-1)<=xy<=(1,1)
                q0_list.append(ci0_tp.orientation_of_xy(xy))
                q1_list.append(ci1_tp.orientation_of_xy(xy))
                pass
            pass
        ci0_q0_xy_list = ci0.xy_of_orientation_list(q0_list)
        ci1_q0_xy_list = ci1.xy_of_orientation_list(q0_list)
        ci0_q1_xy_list = ci0.xy_of_orientation_list(q1_list)
        ci1_q1_xy_list = ci1.xy_of_orientation_list(q1_list)
        off_edge = [0]*4
        in_bounds = True
        for i in range(4):
            for j in range(21):
                (in_bounds, off_edge) = check_bounds(in_bounds, off_edge, i, ci0, ci0_q0_xy_list[i*21+j])
                (in_bounds, off_edge) = check_bounds(in_bounds, off_edge, i, ci1, ci1_q0_xy_list[i*21+j])
                (in_bounds, off_edge) = check_bounds(in_bounds, off_edge, i, ci0, ci0_q1_xy_list[i*21+j])
                (in_bounds, off_edge) = check_bounds(in_bounds, off_edge, i, ci1, ci1_q1_xy_list[i*21+j])
                pass
            pass
        if in_bounds: return None
        return off_edge
    #f overlap_projections
    def overlap_projections(self, images, projections, scale=0.9, max_iter=80):
        ci0 = self.camera_images[images[0]]
        ci1 = self.camera_images[images[1]]
        ci0_tp = projections[0]
        ci1_tp = projections[1]
        for i in range(max_iter):
            overflow = self.find_overflows_for_projections(images, projections)
            size = None
            dxy = [0,0]
            if overflow is None: return projections
            overflow_bits = ( ((overflow[0]>0) and 1) |
                              ((overflow[1]>0) and 2) |
                              ((overflow[2]>0) and 4) |
                              ((overflow[3]>0) and 8) )
            # Edges are 0 => y=-1, 1=> x=1, 2=> y=1, 3=>x=-1
            if overflow_bits in [1, 3,  9, 11]: dxy[1]=1-scale
            if overflow_bits in [4, 6, 12, 14]: dxy[1]=scale-1
            if overflow_bits in [2, 3,  6,  7]: dxy[0]=scale-1
            if overflow_bits in [8, 9, 12, 13]: dxy[0]=1-scale
            if overflow_bits in [5, 7, 13, 10, 11, 14, 15]: size = scale
            if (dxy[0]!=0) or (dxy[1]!=0):
                dxy = tuple(dxy)
                q0 = ci0_tp.orientation_of_xy(dxy)
                q1 = ci0_tp.orientation_of_xy((0,0))
                dq = q1.angle_axis(q0,vector_z)
                ci0_tp.orient(dq*ci0_tp.orientation)
                # now ci0_tp.orientation_of_xy((0,0)).rotate_vector(vector_z) == q0.rotate_vector(vector_z)

                q0 = ci1_tp.orientation_of_xy(dxy)
                q1 = ci1_tp.orientation_of_xy((0,0))
                dq = q1.angle_axis(q0,vector_z)
                ci1_tp.orient(dq*ci1_tp.orientation)

                pass
            if size is not None:
                ci0_tp.focal_length = ci0_tp.focal_length / scale
                ci1_tp.focal_length = ci1_tp.focal_length / scale
                pass
            if False:
                print overflow_bits, size, dxy, ci0_tp.orientation_of_xy((0,0)).rotate_vector(vector_z)
                pass
            pass
        return None
    #f find_matches
    def find_matches(self, images, cxy0=None, size=None, min_cos_sep=min_cos_seps["8pix35"],
                     projections = None):
        #f globals
        global img_png_n

        #b Set to/from projections
        ci0 = self.camera_images[images[0]]
        ci1 = self.camera_images[images[1]]
        if projections is None:
            # 'size' maps to frame_width;
            # fov = 2atan2(frame_width/2focal_length) = 2atan2(size/2)
            # hence size = frame_width/focal_length
            # hence focal_length = frame_width/size
            focal_length = 36.0 / size
            #if img_png_n>0:focal_length=200.0 # GJS T

            src_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")
            dst_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")

            # Currently we can change ci0 orientation and src_to_orientation works great with world_cxy0_q
            # (but presumably not with ci0.orientation() * world_cxy0_q...)
            # The dest projection camera needs to point at world_cxy0_q relative to the destination image
            # as the destination image is accessed 'destination image local'
            # Not sure I buy this logic actually...
            print "Centering on image 0",cxy0, (cxy0[0]+0.5)*5184, (cxy0[1]+0.5)*3456
            world_cxy0_q = ci0.orientation_of_xy(cxy0)

            src_to_orientation = world_cxy0_q
            dst_to_orientation = world_cxy0_q

            dst_img_lp_to.orient(dst_to_orientation)
            src_img_lp_to.orient(src_to_orientation)
            pass

        if projections is not None:
            (src_img_lp_to, dst_img_lp_to) = projections
            pass

        #b Find matches
        self.to_yuv.set_parameters( {"ysc":2.0, "yc":-1.0, "xsc":2.0, "xc":-1.0} )
        self.to_yuv.set_projections(projections=(ci0.lp,src_img_lp_to), num_x_divisions=20, num_y_divisions=20)
        self.to_yuv.execute((ci0.texture,tb[0]))
        self.to_yuv.set_projections(projections=(ci1.lp,dst_img_lp_to), num_x_divisions=20, num_y_divisions=20)
        self.to_yuv.execute((ci1.texture,tb[1]))
        matches = self.im.get_matches(tb)

        save_as_png(tb[0],"a%d.png"%img_png_n)
        save_as_png(tb[1],"b%d.png"%img_png_n)
        img_png_n+=1

        #b Add source -> target matches for qic
        if self.qic is not None:
            for m in matches:
                src_xy = self.xy_from_texture(tb[0],m)
                src_q = src_img_lp_to.orientation_of_xy(src_xy)
                for mm in matches[m]:
                    tgt_xy = self.xy_from_texture(tb[1],mm)
                    tgt_q = dst_img_lp_to.orientation_of_xy(tgt_xy)
                    self.qic.add_match(src_q, tgt_q, mm[2], mm[3], mm[4])
                pass
            return

        #b Add source -> target matches for non-qic
        if self.mappings_by_src_q is None:
            self.mappings_by_src_q = {}
            self.src_qs = []
            pass
        for m in matches:
            src_xy = self.xy_from_texture(tb[0],m)
            src_q = src_img_lp_to.orientation_of_xy(src_xy)
            found_src_q = None
            for src_qx in self.src_qs:
                cos_angle = src_qx.angle_axis(src_q,vector_z).r
                if cos_angle>min_cos_sep:
                    found_src_q = src_qx
                    break
                pass
            if found_src_q is None:
                self.mappings_by_src_q[src_q] = c_mappings(src_xy, src_q)
                self.src_qs.append(src_q)
                pass
            else:
                #print "Found src q",found_src_q,src_q,cos_angle
                src_q = found_src_q
                pass
            for mm in matches[m]:
                tgt_xy = self.xy_from_texture(tb[1],mm)
                fft_rot = 180/math.pi*math.atan2(mm[3],mm[4])
                tgt_q = dst_img_lp_to.orientation_of_xy(tgt_xy)
                fft_power= mm[2]
                self.mappings_by_src_q[src_q].add_match(tgt_xy, tgt_q, fft_rot, fft_power, min_cos_sep=min_cos_sep)
                pass
            pass

        return
    #f create_mappings
    def create_mappings(self):
        if self.qic is not None:
            self.qic.create_mappings()
            return
        #b Build results as dictionary of quaternion matches
        for src_q0 in self.src_qs:
            for src_q1 in self.src_qs:
                if src_q0.id == src_q1.id: continue
                # Look at source distance...
                vector_z = gjslib_c.vector(vector=(0,0,1))
                rq0 = src_q0.angle_axis(src_q1,vector_z)
                rq1 = ~src_q0 * src_q1
                #if rq0.r > 0.999: continue # Skip those less than 2.56 degrees apart
                #if rq0.r > 0.9995: continue # Skip those less than 1.8 degrees apart
                if rq0.r > 0.9999: continue # Skip those less than 0.8 degrees apart
                #if rq0.r > 0.99995: continue # Skip those less than 0.6 degrees apart
                #print "(r=%f,i=%f,j=%f,k=%f) (r=%f,i=%f,j=%f,k=%f)"%(rq0.r,rq0.i,rq0.j,rq0.k, rq1.r,rq1.i,rq1.j,rq1.k)
                for tgt_q0 in self.mappings_by_src_q[src_q0].tgt_qs:
                    for tgt_q1 in self.mappings_by_src_q[src_q1].tgt_qs:
                        qm = c_quaternion_match.test_add(src_qs=(src_q0,src_q1),
                                                         tgt_qs=(tgt_q0,tgt_q1),
                                                         max_angle_diff=0.02) # good matches for 1900/1901 are all <0.02
                        if qm is not None:
                            #rq = qm.src_from_tgt_orient
                            #print "(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k)
                            self.mappings_by_src_q[src_q0].add_tgt_mapping(self.mappings_by_src_q, tgt_q0, src_q1, tgt_q1, qm)
                            pass
                        pass
                    pass
                pass
            pass
        return
    #c c_best_match
    class c_best_match(object):
        unique_id = 0
        qic_qs = {}
        #f qid_from_q
        @classmethod
        def qid_from_q(cls, q, why="none"):
            r = "%f:%f:%f:%f"%(q.r,q.i,q.j,q.k)
            if r in cls.qic_qs: return cls.qic_qs[r][1]
            qic_qid = cls.unique_id
            cls.qic_qs[r]       = (q,qic_qid,why)
            cls.qic_qs[qic_qid] = (q,qic_qid,why)
            cls.unique_id += 1
            return qic_qid
        #f __init__
        def __init__(self, src_from_tgt_q, score, min_cos_sep, max_q_dist):
            self.max_distance = score[0]
            self.min_cos_sep = min_cos_sep
            self.max_q_dist = max_q_dist
            self.src_from_tgt_q = src_from_tgt_q
            self.mappings = score[1]
            self.src_tgts = {} # for each src_q, is a dictionary of (tgt_qs=>counts) that it maps to
            self.qs = {}
            self.best_mappings = []
            self.optimized_src_from_tgt_q = gjslib_c.quaternion(1)
            pass
        #f calculate
        def calculate(self, mappings_by_src_q, qic=None):
            self.src_tgts = {} # for each src_q, is a dictionary of (tgt_qs=>counts) that it maps to
            self.qs = {} # src,tgt,src,tgt -> orientation, distance for those close enough to the vector to count
            self.best_mappings = []
            # create self.src_tgts[src_q][tgt_q] count of mappings for the best src->tgts in score
            if qic:
                for (src_qx,tgt_qx) in self.mappings:
                    self.add_map_qic(src_qx,tgt_qx)
                    src_tgt_mappings = qic.src_tgt_mappings(src_qx, tgt_qx)
                    for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_q) in src_tgt_mappings:
                        dq = self.src_from_tgt_q.distance_to(src_from_tgt_q)
                        if dq>self.max_q_dist: continue
                        self.add_map_qic(src_q1,tgt_q1)
                        src_tgts = (self.qid_from_q(src_qx),
                                    self.qid_from_q(tgt_qx),
                                    self.qid_from_q(src_q1),
                                    self.qid_from_q(tgt_q1),
                                 )
                        # There are no duplicates - this is running throught the best src_qx/tgt_qx mappings that scored
                        #if src_tgts in self.qs: print "Already there"
                        self.qs[src_tgts] = (self.qid_from_q(src_from_tgt_q,"orient"), dq)
                        pass
                    pass
                pass
            else:
                for (src_q,tgt_q) in self.mappings:
                    self.add_map(src_q,tgt_q)
                    for m in mappings_by_src_q[src_q].tgt_q_mappings[tgt_q]:
                        (src_q1, tgt_q1, src_from_tgt_orient) = m
                        dq = self.src_from_tgt_q.distance_to(src_from_tgt_orient)
                        if dq>self.max_q_dist: continue
                        self.add_map(src_q1,tgt_q1)
                        self.qs[(src_q,tgt_q,src_q1,tgt_q1)] = (src_from_tgt_orient, dq)
                        pass
                    pass
                pass
            for src_q in self.src_tgts:
                best_tgt = None
                for tgt_q in self.src_tgts[src_q]:
                    num_matches = self.src_tgts[src_q][tgt_q]
                    if (best_tgt is None) or (num_matches>best_tgt[0]):
                        best_tgt = (num_matches, tgt_q)
                        pass
                    #print "(%d,%d):%d"%(src_q.id, tgt_q.id, num_matches)
                    pass
                if best_tgt is not None:
                    self.best_mappings.append((src_q, best_tgt[1], best_tgt[0]))
                    #print "(%d,%d):%d"%(src_q.id, best_tgt[1].id, best_tgt[0])
                    pass
                pass
            self.best_qs = []
            for (src_q, tgt_q, num_matches) in self.best_mappings:
                #print src_q, tgt_q, num_matches
                for src_tgts in self.qs:
                    (src_q0, tgt_q0, src_q1, tgt_q1) = src_tgts
                    (src_from_tgt_orient, d) = self.qs[src_tgts]
                    if (src_q0==src_q and tgt_q0==tgt_q): # (if src_q1/tgt_q too, then we add them all twice)
                        self.best_qs.append(src_from_tgt_orient)
                        pass
                    pass
                pass
            #self.best_qs.sort()
            #print self.best_qs
            if qic:
                bq = []
                for b in self.best_qs:
                    bq.append(self.__class__.qic_qs[b][0])
                    pass
                self.best_qs=bq
                pass
            #print self.best_qs
            if len(self.best_qs)==0: self.best_qs.append(gjslib_c.quaternion(1))
            self.optimized_src_from_tgt_q = quaternion_average(self.best_qs)
            pass
        #f add_map_qic
        def add_map_qic(self, src_q, tgt_q):
            src_q = self.__class__.qid_from_q(src_q, "src")
            tgt_q = self.__class__.qid_from_q(tgt_q, "tgt")
            if src_q not in self.src_tgts: self.src_tgts[src_q]={}
            if tgt_q not in self.src_tgts[src_q]: self.src_tgts[src_q][tgt_q]=0
            self.src_tgts[src_q][tgt_q] += 1
            pass
        #f add_map
        def add_map(self, src_q, tgt_q):
            if src_q not in self.src_tgts: self.src_tgts[src_q]={}
            if tgt_q not in self.src_tgts[src_q]: self.src_tgts[src_q][tgt_q]=0
            self.src_tgts[src_q][tgt_q] += 1
            pass
        #f old_add_match
        def old_add_match(self, src_q, tgt_q, distance):
            bm.max_distance = -score[0]
            bm.min_cos_sep_score = min_cos_sep_score
            bm.mappings = score[1] # list of src/tgt pairs used
            self.mappings[src_q] = (distance, tgt_q)
            if (self.max_distance is None) or (self.max_distance<distance):
                self.max_distance = distance
                pass
            pass
        pass
    #c c_best_match_qic
    class c_best_match_qic(c_best_match):
        #f __init__
        def __init__(self, src_from_tgt_q, score, qic):
            self.max_distance = score # measure of how good it is - smaller the better
            self.src_from_tgt_q = src_from_tgt_q
            self.min_cos_sep = 0
            self.max_q_dist  = 0
            self.mappings = [] # list of (src/tgt) pairs
            self.best_mappings = [] # list of (src,tgt,n) pairs
            self.qs = {} # map from src,tgt,src,tgt to orientation, distance
            self.qic_scores = qic.scores()
            self.qic_mappings = qic.src_tgt_mappings_of_best_matches(min_score=0, min_count=0)
            self.optimized_src_from_tgt_q = gjslib_c.quaternion(1)
            pass
        #f calculate
        def calculate(self, mappings_by_src_q, qic=None):
            qic_bm_qs = []
            qs = []
            for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_orient) in self.qic_mappings:
                src_q0_id = self.qid_from_q(src_q0,"src")
                tgt_q0_id = self.qid_from_q(tgt_q0,"tgt")
                src_q1_id = self.qid_from_q(src_q1,"src")
                tgt_q1_id = self.qid_from_q(tgt_q1,"tgt")
                qic_bm_qs.append((src_q0_id, tgt_q0_id, src_q1_id, tgt_q1_id, src_from_tgt_orient))
                qs.append(src_from_tgt_orient)
                pass
            self.optimized_src_from_tgt_q = quaternion_average(qs)
            # For save_mappings
            self.qs = {}
            self.best_mappings = []
            self.mappings = []
            for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_orient) in self.qic_mappings:
                src_q0 = self.qid_from_q(src_q0,"src")
                tgt_q0 = self.qid_from_q(tgt_q0,"tgt")
                src_q1 = self.qid_from_q(src_q1,"src")
                tgt_q1 = self.qid_from_q(tgt_q1,"tgt")
                self.mappings.append((src_q0,tgt_q0))
                bm = (src_q0,tgt_q0,0)
                if bm not in self.best_mappings:
                    self.best_mappings.append(bm)
                    pass
                dq = self.src_from_tgt_q.distance_to(src_from_tgt_orient)
                self.qs[(src_q0, tgt_q0, src_q1, tgt_q1)] = (src_from_tgt_orient, dq)
                pass
            pass
    #f find_min_error
    def find_min_error(self, src_from_tgt_q, src_qx):
        return self.mappings_by_src_q[src_qx].find_min_error(src_from_tgt_q, src_qx)
    #f find_error_of_mapping
    def find_error_of_mapping(self, src_from_tgt_q, min_used=10, early_termination=True):
        slack = len(self.mappings_by_src_q)-min_used
        errors = []
        for src_qx in self.src_qs:
            if early_termination and slack<0: return None
            slack = slack-1
            error = self.find_min_error(src_from_tgt_q, src_qx)
            if error is None: continue
            slack += 1
            errors.append( (error[0],src_qx,error[1]) )
            pass
        def cmp_error(x,y):
            if x[0]<y[0]:return -1
            return 1
        errors.sort(cmp=cmp_error)
        for i in range(len(errors)):
            e = errors[i]
            (error, src_qx, tgt_qx) = e
            self.mappings_by_src_q[src_qx].select_for_mapping(src_from_tgt_q, tgt_qx, i, error)
            pass
        if len(errors)<min_used: return None
        total_error = 0
        for e in errors[:min_used]: total_error+=e[0]
        #rq = src_from_tgt_q
        #print total_error,"(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),[e[0] for e in errors[:min_used]]
        return (total_error, errors[:min_used])
    #f score_mapping
    def score_mapping(self, src_from_tgt_q, min_cos_sep=0.9995, max_q_dist=0.02):
        #print "Score mapping",src_from_tgt_q.id
        total_score = (0, [])
        for src_qx in self.src_qs:
            score = self.mappings_by_src_q[src_qx].score_mapping(src_from_tgt_q, min_cos_sep=min_cos_sep, max_q_dist=max_q_dist)
            if score is None: continue
            total_score[1].append((src_qx,score[1]))
            total_score = (total_score[0]+score[0], total_score[1])
            pass
        return total_score
    #f find_best_matches_within_distance
    def find_best_matches_within_distance(self, src_from_tgt_q, src_q_pair=None, min_distance=0.1, min_used=10):
        if src_q_pair:
            (src_q0, src_q1, tgt_q0, tgt_q1) = src_q_pair
            matches_used = self.c_best_match(src_from_tgt_q, src_q0, src_q1, tgt_q0, tgt_q1)
            pass
        else:
            matches_used = self.c_best_match(src_from_tgt_q)
            pass
        slack = len(self.mappings_by_src_q)-min_used
        max_d = None
        for src_qx in self.src_qs:
            if slack<0: return None
            if src_qx in matches_used.mappings: continue
            slack = slack-1
            min_d = (None, None)
            for tgt_qx in self.mappings_by_src_q[src_qx].tgt_qs:
                q = ~src_qx * (src_from_tgt_q*tgt_qx)
                d = math.acos(q.r)
                if (min_d[0] is None) or (min_d[0]>d):
                    min_d = (d, tgt_qx)
                    pass
                pass
            if min_d[0] is None: continue
            if min_d[0]>min_distance: continue
            slack += 1
            matches_used.add_match(src_qx, min_d[1], min_d[0])
            pass
        if len(matches_used.mappings)<min_used: return None
        if matches_used.max_distance is None: return None
        #matches_used.max_distance = matches_used.max_distance/len(matches_used.mappings)
        return matches_used
    #f find_best_target_matches_all
    def find_best_target_matches_all(self, min_used_factor=0.5):
        best_other_target_matches = []
        global_min_d = 1000
        min_used = len(self.mappings_by_src_q)*min_used_factor
        for src_q0 in self.src_qs:
            for tgt_q0 in self.mappings_by_src_q[src_q0].tgt_qs:
                for (src_q1, tgt_q1, src_from_tgt_q) in self.mappings_by_src_q[src_q0].tgt_q_mappings[tgt_q0]:
                    bm = self.find_best_matches_within_distance(src_from_tgt_q, src_q_pair=(src_q0, src_q1, tgt_q0, tgt_q1), min_distance=global_min_d*1.01, min_used=min_used)
                    if bm is None: continue
                    if (bm.max_distance < global_min_d):
                        global_min_d = bm.max_distance
                        pass
                    best_other_target_matches.append(bm)
                    if (len(best_other_target_matches)%10)==0:
                        print "Now have",len(best_other_target_matches),"best matches"
                        rq = bm.src_from_tgt_q
                        if given_src_from_tgt_q: rq = rq * given_src_from_tgt_q
                        print "latest is",bm.max_distance, len(bm.mappings),"(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k)
                        pass
                    pass
                pass
            pass
        return (best_other_target_matches, global_min_d)
    #f find_mappings_to_try
    def find_mappings_to_try(self, min_q_dist=0.0002, max_q_dist=0.5): # default of 0.5 degrees min sep
        #min_used_factor = 0.7
        print "Building list of src_from_tgt_q (min_q_dist %f, max_q_dist %f)"%(min_q_dist,max_q_dist)
        mappings_to_try = []
        mappings_to_try.append(gjslib_c.quaternion(1)) # Ensure identity is on the list
        for src_q0 in self.src_qs:
            for tgt_q0 in self.mappings_by_src_q[src_q0].tgt_qs:
                for (src_q1, tgt_q1, src_from_tgt_q) in self.mappings_by_src_q[src_q0].tgt_q_mappings[tgt_q0]:
                    add_to_list = True
                    for src_from_tgt_qx in mappings_to_try:
                        dq = src_from_tgt_q.distance_to(src_from_tgt_qx)
                        if dq<min_q_dist:
                            add_to_list = False
                            break
                        if dq>max_q_dist:
                            add_to_list = False
                            break
                        pass
                    if not add_to_list: continue
                    mappings_to_try.append(src_from_tgt_q)
                    if (len(mappings_to_try)%500)==0:
                        print "Have %d mappings, continuing"%len(mappings_to_try)
                        pass
                    pass
                pass
            pass
        return mappings_to_try
    #f find_mappings_to_try_qic
    def find_mappings_to_try_qic(self, min_q_dist=0.0002, max_q_dist=0.5): # default of 0.5 degrees min sep
        print "Building from qic list of src_from_tgt_q (min_q_dist %f, max_q_dist %f)"%(min_q_dist,max_q_dist)
        mappings_to_try = []
        mappings_to_try.append(gjslib_c.quaternion(1)) # Ensure identity is on the list
        for src_qx in self.qic.src_qs():
            for tgt_qx in self.qic.tgt_qs_of_src_q(src_qx):
                #print src_qx, tgt_qx
                src_tgt_mappings = self.qic.src_tgt_mappings(src_qx, tgt_qx)
                for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_q) in src_tgt_mappings:
                    #print src_q0, tgt_q0, src_q1, tgt_q1, src_from_tgt_q
                    add_to_list = True
                    for src_from_tgt_qx in mappings_to_try:
                        dq = src_from_tgt_q.distance_to(src_from_tgt_qx)
                        if dq<min_q_dist:
                            add_to_list = False
                            break
                        if dq>max_q_dist:
                            add_to_list = False
                            break
                        pass
                    if not add_to_list: continue
                    mappings_to_try.append(src_from_tgt_q)
                    if (len(mappings_to_try)%500)==0:
                        print "Have %d mappings, continuing"%len(mappings_to_try)
                        pass
                    pass
                pass
            pass
        return mappings_to_try
    #f find_best_target_matches_min_errors
    def find_best_target_matches_min_errors(self, min_used_factor=0.5, min_cos_sep=0.999995):
        mappings_to_try = self.find_mappings_to_try(min_q_dist)
        print "Trying %d mappings (with src/tgts of %d, min_used_factor %f)"%(len(mappings_to_try),len(self.mappings_by_src_q),min_used_factor)
        min_errors = []
        min_used = int(len(self.mappings_by_src_q)*min_used_factor)
        global_min_d = 1000
        first_go = True
        while first_go or min_used>4:
            first_go = False
            for src_from_tgt_q in mappings_to_try:
                error = self.find_error_of_mapping(src_from_tgt_q, min_used=min_used)
                if error is None: continue
                bm = self.c_best_match(src_from_tgt_q)
                bm.max_distance = error[0]
                bm.mappings = error[1]
                min_errors.append(bm)
                pass
            if len(min_errors)>0: break
            new_min_used = min_used*3/4
            if new_min_used==min_used:new_min_used=min_used-1
            min_used = new_min_used
            print "Will try smaller min_used",min_used
            pass
        return (min_errors, global_min_d)

    #f find_best_target_matches
    def find_best_target_matches(self,
                                 min_q_dist=0.00004, # minimum separation betwen src_from_tgt_q's to try
                                 max_q_dist=0.5,
                                 min_cos_sep_score=0.9995,
                                 max_q_dist_score=0.02,
                                 min_cos_sep="Not used", min_used_factor="not used"):
        max_scores = []
        n = 0
        if self.qic is not None:
            cbm = self.c_best_match
            mappings_to_try = self.find_mappings_to_try_qic(min_q_dist, max_q_dist)
            print "Trying %d mappings"%(len(mappings_to_try))
            if False:
                for src_q in self.qic.src_qs():
                    print str(src_q)
                    for tgt_q in self.qic.tgt_qs_of_src_q(src_q):
                        print "  ",str(tgt_q)
                        pass
                    pass
                pass
            for src_from_tgt_q in mappings_to_try:
                score = self.qic.score_orient(src_from_tgt_q)
                if (score>6):#2):
                    score = (score,self.qic.scores())
                    qic_bm_qs = []
                    if False:
                        print score
                        for (src_qx,tgt_qx) in score[1]:
                            src_qx_id = cbm.qid_from_q(src_qx,"src")
                            tgt_qx_id = cbm.qid_from_q(tgt_qx,"tgt")
                            print src_qx_id, tgt_qx_id
                            pass
                        qic_bm = self.qic.best_matches()
                        qic_bm_mappings = {}
                        for (c,src_qx,tgt_qx) in qic_bm:
                            src_qx_id = cbm.qid_from_q(src_qx,"src")
                            tgt_qx_id = cbm.qid_from_q(tgt_qx,"tgt")
                            qic_bm_mappings[(src_qx_id,tgt_qx_id)] = self.qic.src_tgt_mappings(src_qx, tgt_qx)
                            pass
                        for (c,src_qx,tgt_qx) in qic_bm:
                            src_qx_id = cbm.qid_from_q(src_qx,"src")
                            tgt_qx_id = cbm.qid_from_q(tgt_qx,"tgt")
                            for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_orient) in qic_bm_mappings[(src_qx_id,tgt_qx_id)]:
                                src_q1_id = cbm.qid_from_q(src_q1,"src")
                                tgt_q1_id = cbm.qid_from_q(tgt_q1,"tgt")
                                if (src_q1_id, tgt_q1_id) in qic_bm_mappings: # since we are going through all mappings to find the ones we are allowed...
                                    qic_bm_qs.append((src_qx_id, tgt_qx_id, src_q1_id, tgt_q1_id, src_from_tgt_orient))
                                    pass
                                pass
                            pass
                        pass
                    else:
                        for (src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_orient) in self.qic.src_tgt_mappings_of_best_matches(min_score=0, min_count=0):
                            src_q0_id = cbm.qid_from_q(src_q0,"src")
                            tgt_q0_id = cbm.qid_from_q(tgt_q0,"tgt")
                            src_q1_id = cbm.qid_from_q(src_q1,"src")
                            tgt_q1_id = cbm.qid_from_q(tgt_q1,"tgt")
                            qic_bm_qs.append((src_q0_id, tgt_q0_id, src_q1_id, tgt_q1_id, src_from_tgt_orient))
                            pass
                        pass
                    qs = []
                    for q in qic_bm_qs:
                        qs.append(q[4])
                        print q, q[4].distance_to(src_from_tgt_q)
                        pass
                    print quaternion_average(qs)
                    bm = self.c_best_match(src_from_tgt_q, score, min_cos_sep_score, max_q_dist_score)
                    max_scores.append(bm)
                    pass
                pass
            return max_scores
        mappings_to_try = self.find_mappings_to_try(min_q_dist, max_q_dist)
        print "Trying %d mappings (with src/tgts of %d)"%(len(mappings_to_try),len(self.mappings_by_src_q))
        for src_from_tgt_q in mappings_to_try:
            n += 1
            if (n%500)==0:
                print "Trying mapping %d"%n
                pass
            score = self.score_mapping(src_from_tgt_q, min_cos_sep=min_cos_sep_score, max_q_dist=max_q_dist_score)
            if score is None: continue
            if score[0]==0: continue
            bm = self.c_best_match(src_from_tgt_q, score, min_cos_sep_score, max_q_dist_score)
            max_scores.append(bm)
            pass
        return max_scores

    #f find_best_target_matches_qic
    def find_best_target_matches_qic(self,
                                 min_q_dist=0.00004, # minimum separation betwen src_from_tgt_q's to try
                                 max_q_dist=0.5,
                                 min_cos_sep_score=0.9995,
                                 max_q_dist_score=0.02,
                                 min_cos_sep="Not used", min_used_factor="not used"):
        cbm = self.c_best_match
        result = []
        mappings_to_try = self.find_mappings_to_try_qic(min_q_dist, max_q_dist)
        print "Trying %d mappings"%(len(mappings_to_try))
        for src_from_tgt_q in mappings_to_try:
            score = self.qic.score_orient(src_from_tgt_q)
            if (score>2):
                result.append(self.c_best_match_qic(src_from_tgt_q, score, self.qic))
                pass
            pass
        return result
    #f times
    def times(self):
        return {"image_match":self.im.times()}
    #f ALL DONE

#a Testy functions
#f test_to_yuv
def test_to_yuv():
    to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                            extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                           "GL_POSITION":"vec4(ysc*y+yc,xsc*x+xc,0,1)",
                                           })
    img = gjslib_c.texture(filename='images/IMG_2162.JPG')
    to_yuv.f.parameter("ysc",4.0)
    to_yuv.f.parameter("xsc",4.0)
    to_yuv.f.parameter("yc",-2.0)
    to_yuv.f.parameter("xc",-2.0)
    to_yuv.execute((img,tb[4]))
    save_as_png(tb[4],"img_rgb2.png")
    pass

#f test_alu
def test_alu():
    print "ALU tests"

    alu_test( [ ((2,2,0), "vec4(0.5)", "test_constant_50.png"),
                ((2,2,1), "vec4(0.8)", "test_constant_80.png"),
                ((0,1,2), "src_a*src_b", "test_constant_40.png"),
                ] )

    alu_test( [ ((2,2,0), "vec4(x*y)",    "test_xy.png"),
                ((2,2,1), "vec4((1-x)*y)",  "test_x1y.png"),
                ((0,1,2), "vec4(1-4*src_a.r*src_b.r)", "test_xymult.png"),
                ] )

    alu_test( [ ((2,2,0), "vec4(abs(sin(200*x*y)*sin(200*(1-x)*y)))",    "test_sin_xy.png"),
                ] )

#f test_gauss
def test_gauss():
    print "Gauss filter and find test"
    gfx = c_gauss_filter_x()
    gfy = c_gauss_filter_y()
    gfx.execute((tb[0],tb[1]))
    gfy.execute((tb[1],tb[2]))
    save_as_png(tb[2],"test_gauss_sin_xy.png")

    alu_test( [ ((0,2,3),"vec4(30*abs(src_a.r-src_b.r))","test_gauss_sin_xy_diff.png"),
                ] )

    circle_dft = c_circle_dft_filter(extra_defines={"SINGLE_COMPONENT":4, "COMPONENT_SCALE":4})
    circle_dft.execute((tb[2],tb[3]))
    save_as_png(tb[3],"test_gauss_sin_xy_dft.png")

    f = gjslib_c.filter(filter='find:a(10)')
    f.compile()
    f.textures((tb[3],))
    f.parameter("minimum",0.87)
    f.execute()
    print f.num_points
    print f.points

#f test_dft_images
def test_dft_images():
    print "DFT images"

    img = gjslib_c.texture(filename='images/IMG_2162.JPG')
    to_yuv = c_yuv_from_rgb(extra_defines={"INTENSITY_XSCALE":"%f"%(img.height/float(img.width)),
                                           "INTENSITY_YSCALE":"1.0",
                                           "INTENSITY_YOFS":"0.0",
                                           "INTENSITY_XOFS":"0.0",})
    to_yuv.execute((img,tb[4]))
    save_as_png(tb[4],"img_rgb.png")

    for (i,sc) in [(0,1), (1,6), (2,5), (3,3.5), (4,2), (5,2), (6,2)]:
        if False:
                circle_dft = c_circle_dft_filter(extra_defines={"DFT_CIRCLE_RADIUS":4+4*i,
                                                                "SINGLE_COMPONENT":2,
                                                                "COMPONENT_SCALE":4})
                pass
        else:
                circle_dft = c_circle_dft_filter(extra_defines={"DFT_CIRCLE_RADIUS":8,
                                                        "SINGLE_COMPONENT":i,
                                                        "COMPONENT_SCALE":sc})
                pass
        circle_dft.execute((tb[4],tb[5]))
        save_as_png(tb[5],"img_dft_%d.png"%i)
    pass

#f test_diff_image_and_match
def test_diff_image_and_match():
    print "Diff images and map matches"

    quaternion      = gjslib_c.quaternion
    lp_35 = gjslib_c.lens_projection(width=1024.0*5184/3456, frame_width=22.3, focal_length=35.0, lens_type="rectilinear")
    lp_35.orient(quaternion().lookat(vector((0,0,1)),vector((1,0,0))))

    to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                            extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                           "GL_POSITION":"vec4(xsc*x+xc,ysc*y+yc,0,1)",
                                           })
    im = c_image_match()

    imgs = ( gjslib_c.texture(filename='images/IMG_1900.JPG'),
             gjslib_c.texture(filename='images/IMG_1901.JPG') )

    ar = float(imgs[0].width)/imgs[0].height
    to_yuv.set_parameters( {"ysc":2.0, "yc":-1.0, "xsc":2.0*ar, "xc":-1.0*ar} )
    to_yuv.execute((imgs[0],tb[0]))
    to_yuv.execute((imgs[1],tb[1]))
    ic = gjslib_c.image_correlator()
    print "points ",ic.points,"max corners",im.max_corners
    matches = im.get_matches(tb)
    for m in matches:
        print "Match ",m,matches[m]
        ic.add_point(str(m),m[0],m[1])
        n=0
        for mm in matches[m]:
            ic.add_mapping(str(m),"map%s.%d"%(str(m),n),mm[0],mm[1],mm[2],mm[3],mm[4])
            n+=1
            pass
        pass
    print "points ",ic.points
    for pt in ic.points:
        mappings = ic.mappings(pt)
        print "  mapping ",pt,mappings
        for m in mappings:
            print "    ",m,ic.get_mapping(pt,m)    
        pass
    for pt in ic.points:
        print "Num propositions before creation for", pt,  ic.propositions(pt)
    ic.create_propositions(min_strength=0)
    for pt in ic.points:
        n = ic.propositions(pt)
        print "Num propositions after creation for", pt, n
        for i in range(n):
            print "   Proposition",i,ic.get_proposition(pt,i)
            pass
        pass
    best_mapping=ic.find_best_mapping()
    print "Best mapping", best_mapping
    (dx,dy,rotation,scale) = best_mapping[1]
    #(dx,dy,rotation,scale) = (156.4246678601715, -63.62960520116849, 0.22949442458934716, 0.9982186254612156)
    cosang = math.cos(rotation)
    sinang = math.sin(rotation)
    for m in matches:
        (x,y) = m[:2]
        tgt_x = dx+(x*cosang-y*sinang)*scale
        tgt_y = dy+(y*cosang+x*sinang)*scale
        print m[:2],"maps to",(tgt_x,tgt_y)
        pass
    pass

#c c_vector
class c_vector(object):
    #f vector_diff
    @staticmethod
    def vector_diff(v0,v1):
        r = []
        for i in range(len(v0)):
            r.append(v0[i]-v1[i])
            pass
        return tuple(r)
    #f dot_product
    @staticmethod
    def dot_product(p0,p1):
        r = 0
        for i in range(len(p0)):
            r += p0[i]*p1[i]
            pass
        return r
    #f vector_product
    @staticmethod
    def vector_product(p0,p1):
        r = ( p0[1]*p1[2] - p0[2]*p1[1],
              p0[2]*p1[0] - p0[0]*p1[2],
              p0[0]*p1[1] - p0[1]*p1[0])
        return r
    #f vector_length
    @staticmethod
    def vector_length(p):
        l=0
        for d in p: l+=d*d
        return math.sqrt(l)
    #f vector_normalize
    @staticmethod
    def vector_normalize(p):
        l=c_vector.vector_length(p)
        if abs(l)<1E-10:l=1.0
        r=[]
        for d in p: r.append(d/l)
        return tuple(r)
    #f angle_axis_from_v0_to_v1
    @staticmethod
    def angle_axis_from_v0_to_v1(v0,v1):
        axis = v0.cross_product(v1)
        angle = math.atan2(abs(axis),v0.dot_product(v1))
        axis.normalize()
        return (axis,angle)
    #f angle_axis_of_quaternion_diff
    @staticmethod
    def angle_axis_of_quaternion_diff(q0,q1,pt=vector_z):
        return c_vector.angle_axis_from_v0_to_v1(q0.rotate_vector(pt), q1.rotate_vector(pt))

#f validate_buckets
def validate_buckets(results, nbkt=500, fraction=0.01):
    nres = len(results)
    bkts = []
    for i in range(3):
        bkts.append([0]*nbkt)
        pass
    def bkts_of_q(q,nbkt):
        r = []
        qt = v_of_q(q)
        for i in range(3):
            c = qt[i] # -1.0 to 1.0
            b = int((c+1.0)/2.000001*nbkt) # 0 to nbkt
            r.append(b)
            pass
        return r
    for m in results:
        q = results[m]
        qb = bkts_of_q(q,nbkt)
        for i in range(3):
            bkts[i][qb[i]] += 1
        pass
    bkts_small_pct = []
    for i in range(3):
        bkts_small_pct.append([])
        for j in range(nbkt):
            if bkts[i][j] >= (nres*fraction):
                bkts_small_pct[-1].append(j)
                pass
            pass
        pass
    valid_results = {}
    for m in results:
        q = results[m]
        qb = bkts_of_q(q,nbkt)
        valid = True
        for i in range(3):
            if ((qb[i] not in bkts_small_pct[i]) and
                ((qb[i]-1) not in bkts_small_pct[i]) and
                ((qb[i]+1) not in bkts_small_pct[i])):
                valid = False
            pass
        if valid:
            valid_results[m] = q    
        pass
    return valid_results

#f test_diff_image_quaternion
def test_diff_image_quaternion():
    #b Init
    print "Diff images with quaternions"

    quaternion      = gjslib_c.quaternion

    src_orientation = quaternion().from_euler(yaw=0,degrees=True) * quaternion().from_euler(pitch=-30,degrees=True)
    dst_orientation = quaternion().lookat(vector((0,2,1)),vector((0,1,0)))
    #1901
    dst_orientation = quaternion(r=0.950655, i=-0.122744, j=-0.245672, k=-0.144343)
    src_orientation = quaternion(r=0.957717, i=0.023461, j=-0.264079, k=-0.111759)

    dst_orientation = quaternion(r=0.942618, i=0.038782, j=-0.265809, k=-0.198256)
    # This one is best...
    dst_orientation = quaternion(r=0.942579, i=0.039081, j=-0.265648, k=-0.198577)
    # Evening...
    dst_orientation = quaternion(r= 0.942003915584 ,i= 0.0393731632942 ,j= -0.265974639344 ,k= -0.200837915398 )
    dst_orientation = quaternion(r= 0.941906014717 ,i= 0.0389807399661 ,j= -0.266129774404 ,k= -0.201167851624 )
    dst_orientation = quaternion().lookat(vector((0,2,1)),vector((0,1,0)))
    dst_orientation = quaternion(r= 0.943114593009 ,i= 0.036678893399 ,j= -0.266698808114 ,k= -0.195092975228 )
    dst_orientation = src_orientation.copy()

    focal_length = 35.0
    focal_length = 20.0
    src_lp_35 = gjslib_c.lens_projection(width=1024.0, frame_width=22.3*3456/5184, focal_length=focal_length, lens_type="rectilinear")
    dst_lp_35 = gjslib_c.lens_projection(width=1024.0, frame_width=22.3*3456/5184, focal_length=focal_length, lens_type="rectilinear")

    src_lp_35.orient(src_orientation)
    dst_lp_35.orient(dst_orientation)

    #b Find matches
    if True:
        to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                                extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                               "GL_POSITION":"vec4(xsc*x+xc,ysc*y+yc,0,1)",
                                               })
        im = c_image_match()
        im.max_corners=15
        im.max_matches_per_corner=8

        imgs = ( gjslib_c.texture(filename='images/IMG_1901.JPG'),
                 gjslib_c.texture(filename='images/IMG_1902.JPG') )
        imgs = ( gjslib_c.texture(filename='images/IMG_2180.JPG'),
                 gjslib_c.texture(filename='images/IMG_2181.JPG') )

        ar = float(imgs[0].width)/imgs[0].height
        to_yuv.set_parameters( {"ysc":2.0, "yc":-1.0, "xsc":2.0*ar, "xc":-1.0*ar} )
        to_yuv.execute((imgs[0],tb[0]))
        to_yuv.execute((imgs[1],tb[1]))
        matches = im.get_matches(tb)
        print matches
        save_as_png(tb[0],"a.png")
        pass

    #b Matches of stuff
    # Presumably for any two points on source image there is a great circle rotation to go from one to the other
    # The same rotation must match that for the corresponding target positions
    # So, what is the great circle rotation to go from one position to another?
    # We know for each point on an image a quaternion 'q' that maps (0,0,1) to the location in 'image sphere space'
    # Hence we have two points in 'image sphere space' p0 and p1, where pn = qn.(0,0,1).qn'
    # What quaternion maps p0 to p1?
    # The axis of rotation is in direction; p0 x p1; and cos(rot) = p0.p1 (and sin(rot)=|p0 x p1|)
    # Note that p0.p1 = p0 p1 = q0.(0,0,1).q0'.q1'.(0,0,1).q1
    # If q0 = w0,x0,y0,z0, and q1 is w1,x1,y1,z1 then p0.p1 = 4(z0x0-w0y0)(z1x1-w1y1) + 4(w0x0+y0z0)(w1x1+y1z1) + (w0w0-x0x0-y0y0+z0z0)(w1w1-x1x1-y1y1+z1z1)
    # Now if the great circle rotation is represented as a quaternion then its w is the cos of the angle of rotation, i.e. p0.p1
    # Now note also that p0 = q0(0,0,1)
    # Hence q0'(p0) = (0,0,1)
    # Hence q1(q0'(p0)) = p1
    # Hence q1.q0' is the mapping from p0 to p1
    # Now q1.q0' = (w1w0, -w1x0, -w1y0, -w1z0) + i(x1w0, -x1x0, -x1y0, -x1z0) + j(y1w0, -y1x0, -y1y0, -y1z0) + k(z1w0, -z1x0, -z1y0, -z1z0)
    #            = (w1w0, -w1x0, -w1y0, -w1z0) +  (x1x0,  x1w0,  x1z0, -x1y0) +  (y1y0, -y1z0,  y1w0,  y1x0) +  (z1z0,  z1y0, -z1x0,  z1w0)
    #            = (w1w0+x0x1+y0y1+z0z1, x1w0-w1x0+z1y0-y1z0, y1w0-w1y0+x1z0-z1x0, w0z1-w1z0+z0x1-z1x0)
    data = {}
    for m in matches:
        mxy = (m[0]-tb[0].width/2,m[1]-tb[0].height/2)
        print "Match ", m, matches[m],mxy
        src_q = src_lp_35.orientation_of_xy(mxy)
        data[src_q] = (mxy,[])
        for mm in matches[m]:
            mmxy = (mm[0]-tb[1].width/2,mm[1]-tb[1].height/2)
            tgt_q = dst_lp_35.orientation_of_xy(mmxy)
            fft_rot = 180/math.pi*math.atan2(mm[3],mm[4])
            data[src_q][1].append( (tgt_q, fft_rot, mm[2], mmxy) )
            pass
        pass
    #b Show matches
    if False:
        for src_q in data:
            (src_xy, matches) = data[src_q]    
            for (tgt_q, fft_rot, fft_pwr, tgt_xy) in matches:
                print src_xy, "=>", tgt_xy
                pass
            pass
        pass
    #b Build results as dictionary of quaternion matches
    results = {}
    for src_q0 in data:
        for src_q1 in data:
            if src_q0 == src_q1: continue
            for t0 in data[src_q0][1]:
                for t1 in data[src_q1][1]:
                    if abs(t0[0]-t1[0])<1E-8: continue
                    tgt_q0 = t0[0]
                    tgt_q1 = t1[0]
                    qm = c_quaternion_match.test_add(src_qs=(src_q0,src_q1),
                                                     tgt_qs=(tgt_q0,tgt_q1),
                                                     max_angle_diff=0.3)
                    if qm is not None:
                        results[(data[src_q0][0], data[src_q1][0], t0[3], t1[3])] = qm.src_from_tgt_orient * dst_orientation
                        pass
                    pass
                pass
            pass
        pass

    #b Show results
    if False:
        for m in results:
            (s0,s1,t0,t1) = m
            print s0,s1,t0,t1,results[m]
            pass
        pass

    #b Bucket up quaternions to find numerous near-matches
    valid_results = validate_buckets(results)
    print valid_results

    #b Do clusters
    qc = c_quaternion_cluster(valid_results)
    print qc.find_clusters()
    pass

#f test_diff_image_quaternion_2
def test_diff_image_quaternion_2():
    #b Init
    print "Diff images with quaternions (two)"
    imgs = ( gjslib_c.texture(filename='images/IMG_1900.JPG'),
             gjslib_c.texture(filename='images/IMG_1901.JPG') )


    quaternion      = gjslib_c.quaternion

    src_orientation = quaternion(r=1)
    dst_orientation = quaternion(r=1)

    src_img_lp_35 = gjslib_c.lens_projection(width=1024.0, height=1024.0/3456.0*5184, frame_width=22.3, focal_length=35.0, lens_type="rectilinear")
    dst_img_lp_35 = gjslib_c.lens_projection(width=1024.0, height=1024.0/3456.0*5184, frame_width=22.3, focal_length=35.0, lens_type="rectilinear")

    src_img_lp_35.orient(src_orientation)
    dst_orientation = src_orientation * quaternion(r=0.993334,i=-0.006355,j=-0.007261,k=-0.114871) * quaternion(r=0.999925,i=0.006125,j=0.010561,k=-0.000094) * quaternion(r=0.999996,i=-0.002278,j=0.001577,k=0.000149)
    dst_img_lp_35.orient(dst_orientation)

    src_img_lp_to = gjslib_c.lens_projection(width=1024.0, frame_width=36.0, focal_length=250.0, lens_type="rectilinear")
    dst_img_lp_to = gjslib_c.lens_projection(width=1024.0, frame_width=36.0, focal_length=250.0, lens_type="rectilinear")
    src_to_orientation = src_img_lp_35.orientation_of_xy((-0.08,0.25))
    #src_to_orientation = src_img_lp_35.orientation_of_xy((-0.01,0.01))
    dst_to_orientation = dst_img_lp_35.orientation_of_xy((-0.16,0.1))
    dst_to_orientation = dst_orientation * src_to_orientation

    #src_to_orientation = src_img_lp_35.orientation_of_xy((0,0))
    #dst_to_orientation = dst_img_lp_35.orientation_of_xy((0,0))

    src_img_lp_to.orient(src_to_orientation)
    #dst_to_orientation = quaternion(r=0.701862,i=-0.118005,j=-0.043319,k=-0.701133) * src_to_orientation
    #dst_to_orientation = src_to_orientation * quaternion(r=0.993139,i=-0.001478,j=0.003958,k=-0.116867) * dst_to_orientation
    dst_img_lp_to.orient(dst_to_orientation)

    #b Find matches
    if True:
        to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                                extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                               "GL_POSITION":"vec4(xsc*x+xc,ysc*y+yc,0,1)",
                                               })
        im = c_image_match()
        im.max_corners=15
        im.max_matches_per_corner=8
        im.max_corners=40
        im.max_matches_per_corner=6

        to_yuv.set_parameters( {"ysc":2.0, "yc":-1.0, "xsc":2.0, "xc":-1.0} )
        to_yuv.set_projections(projections=(src_img_lp_35,src_img_lp_to), num_x_divisions=20, num_y_divisions=20)
        to_yuv.execute((imgs[0],tb[0]))
        to_yuv.set_projections(projections=(dst_img_lp_35,dst_img_lp_to), num_x_divisions=20, num_y_divisions=20)
        to_yuv.execute((imgs[1],tb[1]))
        matches = im.get_matches(tb)
        print matches
        save_as_png(tb[0],"a.png")
        save_as_png(tb[1],"b.png")
        pass

    #b Matches of stuff
    data = {}
    match_qs = {}
    for m in matches:
        mxy = (m[0]/tb[0].width)*2-1,(m[1]/tb[0].height)*2-1
        print "Match ", m, matches[m], mxy
        src_q = src_img_lp_to.orientation_of_xy(mxy)
        data[src_q] = (mxy,[])
        match_qs[src_q] = []
        for mm in matches[m]:
            mmxy = (mm[0]/tb[1].width)*2-1,(mm[1]-tb[1].height)*2-1
            tgt_q = dst_img_lp_to.orientation_of_xy(mmxy)
            fft_rot = 180/math.pi*math.atan2(mm[3],mm[4])
            data[src_q][1].append( (tgt_q, fft_rot, mm[2], mmxy) )
            match_qs[src_q].append(tgt_q)
            pass
        pass

    #b Build results as dictionary of quaternion matches
    results = {}
    for src_q0 in data:
        for src_q1 in data:
            if src_q0 == src_q1: continue
            for tgt_q0 in match_qs[src_q0]:
                if (src_q0, tgt_q0) not in results:
                    results[(src_q0, tgt_q0)] = []
                    pass
                for tgt_q1 in match_qs[src_q1]:
                    qm = c_quaternion_match.test_add(src_qs=(src_q0,src_q1),
                                                     tgt_qs=(tgt_q0,tgt_q1),
                                                     max_angle_diff=0.3)
                    if qm is not None:
                        results[(src_q0, src_q1, tgt_q0, tgt_q1)] = qm.src_from_tgt_orient
                        results[(src_q0, tgt_q0)].append( (src_q1, tgt_q1) )
                        pass
                    pass
                pass
            pass
        pass

    #b Find 'best other target matches' for each source match
    def blah(match_qs, src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_q, min_distance=0.1):
        matches_used = {src_q0:(0.0,tgt_q0), src_q1:(0.0,tgt_q1)}
        max_d = None
        for src_qx in match_qs:
            if src_qx in matches_used: continue
            min_d = (None, None)
            for tgt_qx in match_qs[src_qx]:
                d = c_vector.vector_diff(v_of_q(src_qx), v_of_q(src_from_tgt_q*tgt_qx))
                d = c_vector.vector_length(d)
                if (min_d[0] is None) or (min_d[0]>d):
                    min_d = (d, tgt_qx)
                    pass
                pass
            if min_d[0] is None: continue
            if min_d[0]>min_distance: continue
            matches_used[src_qx] = min_d
            if (max_d is None) or (max_d<min_d[0]):
                max_d = min_d[0]
            pass
        return (min_d, max_d, matches_used)

    best_other_target_matches = {}
    global_min_d = 1000
    min_used = len(data)/2
    for src_q0 in match_qs:
        for tgt_q0 in match_qs[src_q0]:
            match = (src_q0, tgt_q0)
            if len(results[match])==0:
                continue
            for (src_q1, tgt_q1) in results[match]:
                src_from_tgt_q = results[(src_q0, src_q1, tgt_q0, tgt_q1)]
                (min_d, max_d, matches_used) = blah(match_qs, src_q0, src_q1, tgt_q0, tgt_q1, src_from_tgt_q)
                if max_d is None: continue
                if (max_d < global_min_d) and (len(matches_used)>=min_used):
                    global_min_d = max_d
                    pass
                matches_used["max"] = max_d
                best_other_target_matches[(src_q0, src_q1, tgt_q0, tgt_q1)] = matches_used
                pass
            pass
        pass

    #b Do clusters
    print len(data), global_min_d
    for mapping in best_other_target_matches:
        (src_q0, src_q1, tgt_q0, tgt_q1) = mapping
        #if src_q0 is not data.keys()[0]:continue
        match = best_other_target_matches[mapping]
        if match["max"] > 1.1*global_min_d: continue
        if len(match)<min_used-1: continue
        rq = results[mapping]
        print match["max"], "(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),len(match)
        for sq in match:
            if sq == "max": continue
            if sq is src_q0: continue
            if sq is src_q1: continue
            tgt_qx = match[sq][1]
            txy = "None"
            for t in data[sq][1]:
                if t[0] is tgt_qx:
                    txy = str(t[3])
                    pass
                pass
            print "%6.3f:%s:%s"%(match[sq][0],str(data[sq][0]),txy),
            pass
        print
        pass
    pass

#f test_diff_image_quaternion_3
def test_diff_image_quaternion_3():
    #b Init
    images = ('images/IMG_1901.JPG', 'images/IMG_1902.JPG')
    images = ('images/IMG_1900.JPG', 'images/IMG_1901.JPG')
    focal_length = 35.0
    lens_type="rectilinear"
    initial_dest_orientation = gjslib_c.quaternion(r=1)
    #initial_dest_orientation = gjslib_c.quaternion(r=0.993645,i=0.002668,j=-0.012824,k=-0.111797)
    #initial_dest_orientation = gjslib_c.quaternion(r=0.993378,i=-0.006266,j=-0.007135,k=-0.114499)
    if True:
        images = ('images/IMG_2173.JPG', 'images/IMG_2174.JPG')
        focal_length = 20.5
        lens_type = "rectilinear"
        initial_dest_orientation = gjslib_c.quaternion(r=1)
        # After the initial run, picking up the best match's average from:
        # 71 (r=0.997599,i=-0.005866,j=-0.068908,k=0.003766) 11   7.94:(-0.084691,-0.994923,0.054369)
        #    (r=0.998109,i=-0.008229,j=-0.060531,k=-0.006890)   7.05:(-0.133863,-0.984642,-0.112075)
        initial_dest_orientation = gjslib_c.quaternion(r=0.998109,i=-0.008229,j=-0.060531,k=-0.006890)
        # which yields:
        # 365 (r=0.997460,i=-0.006295,j=-0.068854,k=0.017134) 24   8.17:(-0.088372,-0.966609,0.240536)
        #     (r=0.997090,i=-0.008727,j=-0.071728,k=0.024302)   8.74:(-0.114472,-0.940890,0.318783)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997090,i=-0.008727,j=-0.071728,k=0.024302)
        # which then yield:
        # 743 (r=0.997437,i=-0.009561,j=-0.070331,k=0.009050) 30   8.21:(-0.133625,-0.982928,0.126480)
        #      (r=0.997784,i=-0.006043,j=-0.065743,k=0.008294)   7.63:(-0.090821,-0.988036,0.124643)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997784,i=-0.006043,j=-0.065743,k=0.008294)
        # 611 (r=0.997239,i=-0.005558,j=-0.070579,k=0.022411) 28   8.52:(-0.074841,-0.950434,0.301786)
        #     (r=0.997335,i=-0.006231,j=-0.070523,k=0.017625)   8.37:(-0.085409,-0.966617,0.241572)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997335,i=-0.006231,j=-0.070523,k=0.017625)
        # 634 (r=0.997335,i=-0.006231,j=-0.070523,k=0.017625) 26   8.37:(-0.085405,-0.966616,0.241575)
        #     (r=0.997314,i=-0.006328,j=-0.071135,k=0.016289)   8.40:(-0.086388,-0.971125,0.222380)
        pass
    ipqm = c_image_pair_quaternion_match()
    ipqm.add_image(image_filename=images[0], orientation=gjslib_c.quaternion(r=1), focal_length=focal_length, lens_type=lens_type )
    ipqm.add_image(image_filename=images[1], orientation=initial_dest_orientation, focal_length=focal_length, lens_type=lens_type )

    print "Diff images with quaternions (three)"

    focal_length = 50.0
    ci0 = ipqm.camera_images[images[0]]
    ci1 = ipqm.camera_images[images[1]]

    ipqm.mappings_by_src_q = None
    for initial_orientation in [gjslib_c.quaternion().from_euler(yaw=-12.0,degrees=1),
                                gjslib_c.quaternion().from_euler(yaw=+12.0,degrees=1),
                                ]:
        src_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")
        dst_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")
        src_img_lp_to.orient(initial_orientation)
        dst_img_lp_to.orient(initial_orientation)
        projections = (src_img_lp_to, dst_img_lp_to)
        if not ipqm.overlap_projections((images[0], images[1]), (src_img_lp_to, dst_img_lp_to)):
            raise Exception, "Failed to find overlapping projection"
        print "Using src projection focal length",src_img_lp_to.focal_length,"centered on",(src_img_lp_to.orientation).rotate_vector(vector_z)
        ipqm.find_matches((images[0], images[1]), projections=projections)
        pass

    ipqm.create_mappings()
    best_other_target_matches = ipqm.find_best_target_matches(max_q_dist = 2, # All directions - actually should be FOV, so about 45 degrees
                                                              min_q_dist        = min_q_dists["80pix35"], # min separation between src_from_tgt_q's to try to score
                                                              min_cos_sep       = min_cos_seps["80pix35"],
                                                              min_cos_sep_score = min_cos_seps["80pix35"],
                                                              max_q_dist_score  = min_q_dists["80pix35"],
                                                              )

    #b Do clusters
    def cmp_matches(x,y):
        return cmp(y.max_distance,x.max_distance)
        if x.max_distance/len(x.mappings)<y.max_distance/len(y.mappings): return -1
        return 1

    best_other_target_matches.sort(cmp=cmp_matches)
    print "Best matches for whole image"
    max_n = len(best_other_target_matches)
    if max_n>20:max_n=20
    for mapping in best_other_target_matches[:max_n]:
        mapping.calculate(ipqm.mappings_by_src_q)
        rq = mapping.src_from_tgt_q * initial_dest_orientation
        print mapping.max_distance, "(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),len(mapping.mappings),rq.to_rotation_str(1)
        rq = mapping.optimized_src_from_tgt_q * initial_dest_orientation
        print "     (r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),rq.to_rotation_str(1)
        pass

    c_mappings.save_mappings(ipqm.mappings_by_src_q, open("image_mappings.py","w"), initial_dest_orientation, best_other_target_matches)

    pass

#f test_diff_image_quaternion_4
def test_diff_image_quaternion_4():
    #b Init
    images = ('images/IMG_1901.JPG', 'images/IMG_1902.JPG')
    images = ('images/IMG_1900.JPG', 'images/IMG_1901.JPG')
    focal_length = 35.0
    lens_type="rectilinear"
    initial_dest_orientation = gjslib_c.quaternion(r=1)
    #initial_dest_orientation = gjslib_c.quaternion(r=0.993645,i=0.002668,j=-0.012824,k=-0.111797)
    #initial_dest_orientation = gjslib_c.quaternion(r=0.993378,i=-0.006266,j=-0.007135,k=-0.114499)
    if True:
        images = ('images/IMG_2173.JPG', 'images/IMG_2174.JPG')
        focal_length = 20.5
        lens_type = "rectilinear"
        initial_dest_orientation = gjslib_c.quaternion(r=1)
        ## With QIC:
        # 39.0 (r=0.997599,i=-0.005866,j=-0.068908,k=0.003766) 39   7.94:(-0.084691,-0.994923,0.054369)
        #      (r=0.997459,i=-0.006535,j=-0.070060,k=0.011136)   8.17:(-0.091726,-0.983439,0.156313)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997599,i=-0.005866,j=-0.068908,k=0.003766)
        # 192.0 (r=0.997242,i=-0.005395,j=-0.070274,k=0.023291) 33   8.51:(-0.072675,-0.946714,0.313769)
        #       (r=0.997407,i=-0.005391,j=-0.069664,k=0.017243)   8.25:(-0.074912,-0.967979,0.239595)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997242,i=-0.005395,j=-0.070274,k=0.023291)
        # 249.0 (r=0.997242,i=-0.005395,j=-0.070274,k=0.023291) 33   8.51:(-0.072680,-0.946713,0.313770)
        #       (r=0.997349,i=-0.005712,j=-0.070469,k=0.017270)   8.35:(-0.078484,-0.968263,0.237291)
        initial_dest_orientation = gjslib_c.quaternion(r=0.997242,i=-0.005395,j=-0.070274,k=0.023291)
        ## Without QIC:
        ## After the initial run, picking up the best match's average from:
        ## 71 (r=0.997599,i=-0.005866,j=-0.068908,k=0.003766) 11   7.94:(-0.084691,-0.994923,0.054369)
        ##    (r=0.998109,i=-0.008229,j=-0.060531,k=-0.006890)   7.05:(-0.133863,-0.984642,-0.112075)
        #initial_dest_orientation = gjslib_c.quaternion(r=0.998109,i=-0.008229,j=-0.060531,k=-0.006890)
        ## which yields:
        ## 365 (r=0.997460,i=-0.006295,j=-0.068854,k=0.017134) 24   8.17:(-0.088372,-0.966609,0.240536)
        ##     (r=0.997090,i=-0.008727,j=-0.071728,k=0.024302)   8.74:(-0.114472,-0.940890,0.318783)
        #initial_dest_orientation = gjslib_c.quaternion(r=0.997090,i=-0.008727,j=-0.071728,k=0.024302)
        ## which then yield:
        ## 743 (r=0.997437,i=-0.009561,j=-0.070331,k=0.009050) 30   8.21:(-0.133625,-0.982928,0.126480)
        ##      (r=0.997784,i=-0.006043,j=-0.065743,k=0.008294)   7.63:(-0.090821,-0.988036,0.124643)
        #initial_dest_orientation = gjslib_c.quaternion(r=0.997784,i=-0.006043,j=-0.065743,k=0.008294)
        ## 611 (r=0.997239,i=-0.005558,j=-0.070579,k=0.022411) 28   8.52:(-0.074841,-0.950434,0.301786)
        ##     (r=0.997335,i=-0.006231,j=-0.070523,k=0.017625)   8.37:(-0.085409,-0.966617,0.241572)
        #initial_dest_orientation = gjslib_c.quaternion(r=0.997335,i=-0.006231,j=-0.070523,k=0.017625)
        ## 634 (r=0.997335,i=-0.006231,j=-0.070523,k=0.017625) 26   8.37:(-0.085405,-0.966616,0.241575)
        ##     (r=0.997314,i=-0.006328,j=-0.071135,k=0.016289)   8.40:(-0.086388,-0.971125,0.222380)

        pass
    ipqm = c_image_pair_quaternion_match()
    ipqm.add_image(image_filename=images[0], orientation=gjslib_c.quaternion(r=1), focal_length=focal_length, lens_type=lens_type )
    ipqm.add_image(image_filename=images[1], orientation=initial_dest_orientation, focal_length=focal_length, lens_type=lens_type )
    quaternion_image_correlate(ipqm, images, initial_dest_orientation)
    pass

#f quaternion_image_correlate
def quaternion_image_correlate(ipqm, images, initial_dest_orientation):

    print "Quaternion_Image_Correlate"

    focal_length = 50.0
    ci0 = ipqm.camera_images[images[0]]
    ci1 = ipqm.camera_images[images[1]]

    ipqm.qic = gjslib_c.quaternion_image_correlator()
    for initial_orientation in [gjslib_c.quaternion().from_euler(yaw=-12.0,degrees=1),
                                gjslib_c.quaternion().from_euler(yaw=+12.0,degrees=1),
                                ]:
        src_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")
        dst_img_lp_to = gjslib_c.lens_projection(width=2.0, height=2.0, frame_width=36.0, focal_length=focal_length, lens_type="rectilinear")
        src_img_lp_to.orient(initial_orientation)
        dst_img_lp_to.orient(initial_orientation)
        projections = (src_img_lp_to, dst_img_lp_to)
        if not ipqm.overlap_projections((images[0], images[1]), (src_img_lp_to, dst_img_lp_to)):
            raise Exception, "Failed to find overlapping projection"
        print "Using src projection focal length",src_img_lp_to.focal_length,"centered on",(src_img_lp_to.orientation).rotate_vector(vector_z)
        ipqm.find_matches((images[0], images[1]), projections=projections)
        pass

    ipqm.qic.create_mappings()
    best_other_target_matches = ipqm.find_best_target_matches_qic(max_q_dist = 2, # All directions - actually should be FOV, so about 45 degrees
                                                                  min_q_dist        = min_q_dists["80pix35"], # min separation between src_from_tgt_q's to try to score
                                                                  min_cos_sep       = min_cos_seps["80pix35"],
                                                                  min_cos_sep_score = min_cos_seps["80pix35"],
                                                                  max_q_dist_score  = min_q_dists["80pix35"],
                                                                  )
    print ipqm.times()

    #b Do clusters
    def cmp_matches(x,y):
        return cmp(y.max_distance,x.max_distance)
        if x.max_distance/len(x.mappings)<y.max_distance/len(y.mappings): return -1
        return 1

    best_other_target_matches.sort(cmp=cmp_matches)
    print "Best matches for whole image"
    max_n = len(best_other_target_matches)
    if max_n>20:max_n=20
    for mapping in best_other_target_matches[:max_n]:
        mapping.calculate(ipqm.mappings_by_src_q, ipqm.qic)
        rq = mapping.src_from_tgt_q * initial_dest_orientation
        print mapping.max_distance, "(r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),len(mapping.mappings),rq.to_rotation_str(1)
        rq = mapping.optimized_src_from_tgt_q * initial_dest_orientation
        print "     (r=%f,i=%f,j=%f,k=%f)"%(rq.r,rq.i,rq.j,rq.k),rq.to_rotation_str(1)
        pass

    c_mappings.save_mappings(ipqm.mappings_by_src_q, open("image_mappings.py","w"), initial_dest_orientation, best_other_target_matches, qic=ipqm.qic)

    pass

#f test_diff_image
def test_diff_image():
    print "Diff images"

    imgs = ( gjslib_c.texture(filename='images/IMG_1900.JPG'),
             gjslib_c.texture(filename='images/IMG_1901_r10.JPG') )

    to_yuv = c_yuv_from_rgb(extra_parameters={"xsc":2.0,"ysc":2.0,"xc":1.0,"yc":1.0},
                            extra_defines={"EXTRA_VERTEX_UNIFORMS":"uniform float xsc, ysc, xc, yc;",
                                           "GL_POSITION":"vec4(xsc*x+xc,ysc*y+yc,0,1)",
                                           })
    copy_img = c_alu_filter(extra_defines={"OP":"src_a"})
    equalize = c_windowed_equalization_filter()
    harris = c_harris_filter()
    find_corners = c_find_filter(extra_parameters={"min_distance":10.0, "max_elements":20})
    find_matches = c_find_filter(extra_parameters={"min_distance":2.5, "minimum":0.1, "max_elements":100})
    circle_dft = c_circle_dft_filter(extra_defines={"DFT_CIRCLE_RADIUS":8,
                                                    "CIRCLE_COMPONENT":"r",
                                                    })
    circle_dft_diff         = c_circle_dft_diff_filter()
    circle_dft_diff_combine = c_circle_dft_diff_combine_filter()

    ar = float(imgs[0].width)/imgs[0].height
    to_yuv.set_parameters( {"ysc":2.0, "yc":-1.0, "xsc":2.0*ar, "xc":-1.0-ar*0} )
    to_yuv.execute((imgs[0],tb[0]))
    to_yuv.execute((imgs[1],tb[1]))
    harris.execute( (tb[0],tb[4]) )

    if False: # do windowed equalization to remove brightness and contrastiness dependence - with loss of information
        copy_img.execute((tb[0],tb[0],tb[2]))
        equalize.execute((tb[2],tb[0]))
        copy_img.execute((tb[1],tb[1],tb[2]))
        equalize.execute((tb[2],tb[1]))
        pass
    circle_dft.execute((tb[0],tb[2]))
    circle_dft.execute((tb[1],tb[3]))
    find_corners.execute( (tb[4],) )

    quaternion      = gjslib_c.quaternion
    lp_20 = gjslib_c.lens_projection(width=1024.0, frame_width=36.0, focal_length=20.0, lens_type="rectilinear")
    lp_20.orient(quaternion().lookat(vector((0,0,1)),vector((1,0,0))))
    print find_corners.f.num_points
    corners = find_corners.f.points[:3]
    matches = {}
    for i in range(len(corners)):
        pt = corners[i]
        xy = (pt[0],pt[1])
        for i, dxy in [(0,(4,0)), (1,(0,4)), (2,(-4,0)), (3,(0,-4))]:
            circle_dft_diff.set_parameters( {"uv_base_x":xy[0]+dxy[0], "uv_base_y":xy[1]+dxy[1]} )
            circle_dft_diff.execute( (tb[2], tb[3], tb[5+i]) )
            pass
        circle_dft_diff_combine.execute( (tb[5], tb[6], tb[7], tb[8], tb[9]) )
        find_matches.execute( (tb[9],) )
        matches[xy] = find_matches.f.points[0]
        pass
    def q_inner_product(q1,q2):
        s = 0
        for i in range(4):
            s += q1.rijk[i]*q2.rijk[i]
            pass
        a1 = q1.rotate_vector(vector_z)
        a2 = q2.rotate_vector(vector_z)
        s = 0
        for i in range(3):
            s += a1[i]*a2[i]
        return s
    matches[(512,512)]=(500,582)
    for xy in matches.keys():
        print xy," -> ",matches[xy]
        src_q = lp_20.orientation_of_xy((xy[0]-512,xy[1]-512))
        dst_q = lp_20.orientation_of_xy((matches[xy][0]-512,matches[xy][1]-512))
        print src_q, dst_q
        q1q2 = q_inner_product(src_q,dst_q)
        print q1q2, 180/math.pi*math.acos(q1q2)
        #print 2*q1q2*q1q2-1, 180/math.pi*math.acos(2*q1q2*q1q2-1)
        pass
    if False:
        for i in range(10):
            save_as_png(tb[i],"img_dft_%d.png"%i)
            pass
        pass

    pass

#f test_mandel_projection
def test_mandel_projection():
    lens_projection = gjslib_c.lens_projection
    quaternion      = gjslib_c.quaternion
    vector          = gjslib_c.vector

    lp_from = lens_projection(width=2.0, frame_width=36.0, focal_length=40.0, lens_type="equiangular")
    lp_to = lens_projection(width=2.0, frame_width=36.0, focal_length=10.0, lens_type="rectilinear")
    lp_from.orient(quaternion().lookat(vector((0,0,1)),vector((1,0,0))))
    lp_to.orient(quaternion().lookat(vector((0.3,0.3,1)),vector((1,0,0))))

    mandel = c_mandelbrot_filter()
    mandel.execute( (tb[1],tb[0]) )
    #alu_test( [ ((2,2,0), "vec4(abs(sin(200*x*y)*sin(200*(1-x)*y)))",    "test_sin_xy.png"),
    #            ] )

    copy_img = c_alu_filter(extra_defines={"OP":"src_a"})
    copy_img.set_projections(projections=(lp_from,lp_to), num_x_divisions=20, num_y_divisions=20)
    copy_img.execute((tb[0],tb[1],tb[2]))
    copy_img.set_projections(projections=(lp_to,lp_from), num_x_divisions=20, num_y_divisions=20)
    copy_img.execute((tb[2],tb[0],tb[1]))
    for i in range(3):
        save_as_png(tb[i],"img_proj_%d.png"%i)

#f test_lens_projection
def test_lens_projection():
    lens_projection = gjslib_c.lens_projection
    quaternion      = gjslib_c.quaternion

    lp_20 = lens_projection(width=2.0, frame_width=36.0, focal_length=20.0, lens_type="rectilinear")
    lp_30 = lens_projection(width=2.0, frame_width=36.0, focal_length=30.0, lens_type="rectilinear")
    lp_20.orient(quaternion().lookat(vector((0,0,1)),vector((1,0,0))))
    lp_30.orient(quaternion().lookat(vector((0,0,1)),vector((1,0,0))))

    a = quaternion.roll(60, degrees=1)
    print a.rotate_vector(vector(1,0,3))
    print a.rotate_vector(vector(0,1,3))

    lp_20 = lens_projection(width=2.0, frame_width=36.0, focal_length=20.0, lens_type="rectilinear")
    print quaternion.roll(60,degrees=1)
    print quaternion().identity()
    a = quaternion()
    b = quaternion()
    c = quaternion(r=1.0)
    print c
    c = quaternion(r=1, i=2, j=3, k=4) * quaternion(i=1)* quaternion(j=1)* quaternion(k=1)
    print c

    a.scale(2)
    print a.rijk
    a.lookat(vector((1,2,3)),vector((4,5,4)))
    b.lookat(vector((1,0,0)),vector((0,1,0)))
    print "a", a
    print "b", b
    print "~a", ~a
    print abs(a)
    a.from_rotation((1,1,0),60.0,degrees=1)
    print a
    a.from_euler(pitch=60.0,degrees=1)
    lp_20.orient(a)
    print lp_20
    print "orientation of 0,0",lp_20.orientation_of_xy((0,0))
    print "x,y of orientation of a",lp_20.xy_of_orientation(a)
    a.from_rotation((1,1,0),60.0,degrees=1)
    lp_20.orient(a)
    print lp_20
    print "orientation of 0,0",lp_20.orientation_of_xy((0,0))
    print "x,y of orientation of a",lp_20.xy_of_orientation(a)
    print a
    print lp_20.orientation
    a.scale(2)
    print a
    print lp_20.orientation
    pass

#f test_quaternion_rotation_z
def test_quaternion_rotation_z():
    quaternion = gjslib_c.quaternion
    for w in range(4):
        for x in range(4):
            for y in range(4):
               for z in range(4):
                   q = quaternion(r=w,i=x,j=y,k=z)
                   r = q.rotate_vector(vector(0,0,1))
                   r2 = (2*(z*x-w*y), 2*(w*x+y*z), w*w-x*x-y*y+z*z)
                   for i in range(3):
                       if abs(r[i]-r2[i])>0.000001:
                           print "Error in quaternion rotation"
                           pass
                       pass
                   pass
               pass
            pass
        pass
    pass

#a Toplevel
if False:
    test_to_yuv()

if False:
    test_alu()

if False:
    test_gauss()

if False:
    test_dft_images()

if False:
    test_diff_image()

if False:
    test_diff_image_and_match()

if False:
    test_mandel_projection()

if False:
    test_lens_projection()

if False:
    test_diff_image_quaternion()

if False:
    test_diff_image_quaternion_2()

if False:
    test_diff_image_quaternion_3()

if True:
    test_diff_image_quaternion_4()