class AlgorithmBase:
    def __init__(self, system_model, render_engine, obj_idx):
        self.system_model = system_model
        self.render_engine = render_engine
        self.obj_idx = obj_idx
        self.debug_filebase = None
        self.timer = None
        self._cam = self.system_model.cam

        self.latest_discretization_err_q = None
        self.latest_discretization_light_err_angle = None

        # initialize by the call to set_image_zoom_and_resolution
        self.im_xoff = None
        self.im_yoff = None
        self.im_width = None
        self.im_height = None
        self.im_def_scale = None
        self.im_scale = None

        self.set_image_zoom_and_resolution()

    def set_image_zoom_and_resolution(self, im_xoff=0, im_yoff=0, im_width=None, im_height=None):
        self.im_xoff = im_xoff
        self.im_yoff = im_yoff
        self.im_width = im_width or self._cam.width
        self.im_height = im_height or self._cam.height
        self.im_def_scale = self.system_model.view_width/self.im_width
        self.im_scale = self.im_def_scale

        # calculate frustum based on fov, aspect & near
        # NOTE: with wide angle camera, would need to take into account
        #       im_xoff, im_yoff, im_width and im_height
        x_fov = self._cam.x_fov * self.im_def_scale / self.im_scale
        y_fov = self._cam.y_fov * self.im_def_scale / self.im_scale
        self.render_engine.set_frustum(x_fov, y_fov, self.system_model.min_altitude*.1, self.system_model.max_distance)

    def load_target_image(self, src):
        tmp = cv2.imread(src, cv2.IMREAD_GRAYSCALE)
        if tmp is None:
            raise Exception('Cant load image from file %s' % (src,))

        if tmp.shape != (self._cam.height, self._cam.width):
            # visit fails to generate 1024 high images
            tmp = cv2.resize(tmp, None,
                             fx=self._cam.width / tmp.shape[1],
                             fy=self._cam.height / tmp.shape[0],
                             interpolation=cv2.INTER_CUBIC)
        return tmp

    def remove_background(self, img):
        res_img, h, th = ImageProc.process_target_image(img)
        return res_img, th

    def load_obj(self, obj_file, obj_idx=None):
        self.obj_idx = self.render_engine.load_object(obj_file, obj_idx)

    def render(self, center=False, depth=False, discretize_tol=False, shadows=True, textures=True,
               gamma=1.8, reflection=RenderEngine.REFLMOD_LUNAR_LAMBERT):
        assert not discretize_tol, 'discretize_tol deprecated at render function'

        rel_pos_v, rel_rot_q, light_v = self._render_params(discretize_tol, center)
        RenderEngine.REFLMOD_PARAMS[reflection] = self.system_model.asteroid.reflmod_params[reflection]
        res = self.render_engine.render(self.obj_idx, rel_pos_v, rel_rot_q, light_v, get_depth=depth,
                                        shadows=shadows, textures=textures,
                                        gamma=gamma, reflection=reflection)
        if depth:
            img, dth = res
        else:
            img = res

        return (img, dth) if depth else img


    def _render_params(self, discretize_tol=False, center_model=False):
        assert not discretize_tol, 'discretize_tol deprecated at _render_params function'

        m = self.system_model

        # NOTE: with wide angle camera, would need to take into account
        #       im_xoff, im_yoff, im_width and im_height
        xc_off = (self.im_xoff + self.im_width / 2 - self._cam.width / 2)
        xc_angle = xc_off / self._cam.width * math.radians(self._cam.x_fov)

        yc_off = (self.im_yoff + self.im_height / 2 - self._cam.height / 2)
        yc_angle = yc_off / self._cam.height * math.radians(self._cam.y_fov)

        # first rotate around x-axis, then y-axis,
        # note that diff angle in image y direction corresponds to rotation
        # around x-axis and vise versa
        q_crop = (
                np.quaternion(math.cos(-yc_angle / 2), math.sin(-yc_angle / 2), 0, 0)
                * np.quaternion(math.cos(-xc_angle / 2), 0, math.sin(-xc_angle / 2), 0)
        )

        x = m.x_off.value
        y = m.y_off.value
        z = m.z_off.value

        # rotate offsets using q_crop
        x, y, z = tools.q_times_v(q_crop.conj(), np.array([x, y, z]))

        # maybe put object in center of view
        if center_model:
            x, y = 0, 0

        # get object rotation and turn it a bit based on cropping effect
        q, err_q = m.gl_sc_asteroid_rel_q(discretize_tol)
        sc2gl_q = m.frm_conv_q(m.SPACECRAFT_FRAME, m.OPENGL_FRAME)
        self.latest_discretization_err_q = sc2gl_q * err_q * sc2gl_q.conj() if discretize_tol else False

        qfin = (q * q_crop.conj())

        # light direction
        light, err_angle = m.gl_light_rel_dir(err_q, discretize_tol)
        self.latest_discretization_light_err_angle = err_angle if discretize_tol else False

        return (x, y, z), qfin, light
 class RosettaSystemModel(SystemModel):
    def __init__(self, hi_res_shape_model=False, rosetta_batch='mtp006', focused_attenuated=True, res_mult=1.0):
        # gives some unnecessary warning about "dubious year" even when trying to ignore it
        with warnings.catch_warnings():
            warnings.simplefilter("ignore")
            min_time = Time('2015-01-01 00:00:00', scale='utc', format='iso')

        fa = focused_attenuated  # else defocused not attenuated
        super(RosettaSystemModel, self).__init__(
            asteroid=ChuryumovGerasimenko(hi_res_shape_model=hi_res_shape_model, rosetta_batch=rosetta_batch),

            # see https://pds-smallbodies.astro.umd.edu/holdings/ro-c-navcam-2-esc4-mtp023-v1.0/document/ro-sgs-if-0001.pdf
            camera=Camera(
                int(1024*res_mult),       # width in pixels
                int(1024*res_mult),       # height in pixels
                5,          # x fov in degrees
                5,          # y fov in degrees
                focal_length=152.5,  # in mm
                # sensor_size=(1024*0.013, 1024*0.013),
                aperture=30 if fa else 70,    # attenuated mode (in non-attenuated mode would be 70mm)
                # f_stop=5.1,     # attenuated mode (in non-attenuated mode would be 2.2)
                quantum_eff=0.80,     # from https://www.e2v.com/resources/account/download-datasheet/1427
                px_saturation_e=1e5,  # same source as above
                # gain can be 1.0 (low) or 1.7 (high)   # from https://pds-smallbodies.astro.umd.edu/holdings/ro-c-navcam-2-esc4-mtp023-v1.0/document/ro-sgs-if-0001.pdf
                lambda_min=500e-9, lambda_eff=650e-9, lambda_max=800e-9,   # for bandwidth calc
                dark_noise_mu=250, dark_noise_sd=60, readout_noise_sd=2,    # noise params (e-/s, e-)
                point_spread_fn=0.65 if fa else 0.25,       # 0.50-0.55 for defocused, 0.65-0.70 for focused
                scattering_coef=5e-9,  # affects strength of haze/veil when sun shines on the lens (TODO: just a guess now)
                exclusion_angle_x=15,
                exclusion_angle_y=15,

                # 30mm aperture and attenuation filter result in an attenuation factor of ∼580 relative to 70mm aperture and no filter
                #   => if aperture is 30, need extra attenuation coef: x*(30/70)**2==1/580 => x==0.009387
                # empirical coef used to tune the synthetic images to match real brightnesses (includes attenuation filter effect)
                #  - based on star brightnesses, note that point_spread_fn affects this a lot
                emp_coef=(0.009387 * 2.3 if fa else 2.3),
            ),
            limits=(
                25,  # min_distance in km
                70,  # min_med_distance in km
                400,  # max_med_distance in km #640
                1250,  # max_distance in km
                40,  # min_elong in deg
                min_time,  # min time instant
            )
        )
        self.mission_id = 'rose'
 class RenderEngine:
    _ctx = None

    (
        _LOC_TEXTURE,
        _LOC_SHADOW_MAP,
        _LOC_HAPKE_K,
    ) = range(3)

    (
        REFLMOD_LAMBERT,
        REFLMOD_LUNAR_LAMBERT,
        REFLMOD_HAPKE,
    ) = range(3)

    REFLMOD_PARAMS = {
        REFLMOD_LAMBERT: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        REFLMOD_LUNAR_LAMBERT: ChuryumovGerasimenko.LUNAR_LAMBERT_PARAMS,
        REFLMOD_HAPKE: ChuryumovGerasimenko.HAPKE_PARAMS,
    }

    # phase angle (g) range: np.linspace(0, 180, 19)
    # roughness angle range (th_p) range: np.linspace(0, 60, 7)
    HAPKE_K = np.array([
        [1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00],
#        [1.00, 0.997, 0.991, 0.984, 0.974, 0.961, 0.943],  # g=2deg
#        [1.00, 0.994, 0.981, 0.965, 0.944, 0.918, 0.881],  # g=5deg
        [1.00, 0.991, 0.970, 0.943, 0.909, 0.866, 0.809],
        [1.00, 0.988, 0.957, 0.914, 0.861, 0.797, 0.715],
        [1.00, 0.986, 0.947, 0.892, 0.825, 0.744, 0.644],
        [1.00, 0.984, 0.938, 0.871, 0.789, 0.692, 0.577],
        [1.00, 0.982, 0.926, 0.846, 0.748, 0.635, 0.509],
        [1.00, 0.979, 0.911, 0.814, 0.698, 0.570, 0.438],
        [1.00, 0.974, 0.891, 0.772, 0.637, 0.499, 0.366],
        [1.00, 0.968, 0.864, 0.719, 0.566, 0.423, 0.296],
        [1.00, 0.959, 0.827, 0.654, 0.487, 0.346, 0.231],
        [1.00, 0.946, 0.777, 0.575, 0.403, 0.273, 0.175],
        [1.00, 0.926, 0.708, 0.484, 0.320, 0.208, 0.130],
        [1.00, 0.894, 0.617, 0.386, 0.243, 0.153, 0.094],
        [1.00, 0.840, 0.503, 0.290, 0.175, 0.107, 0.064],
        [1.00, 0.747, 0.374, 0.201, 0.117, 0.070, 0.041],
        [1.00, 0.590, 0.244, 0.123, 0.069, 0.040, 0.023],
        [1.00, 0.366, 0.127, 0.060, 0.032, 0.018, 0.010],
        [1.00, 0.128, 0.037, 0.016, 0.0085, 0.0047, 0.0026],
        [1.00, 0, 0, 0, 0, 0, 0],
    ]).T

    def __init__(self, view_width, view_height, antialias_samples=0, enable_extra_data=False):
        if RenderEngine._ctx is None:
            RenderEngine._ctx = moderngl.create_standalone_context()

        self._ctx = RenderEngine._ctx
        self._width = view_width
        self._height = view_height
        self._samples = antialias_samples
        self._enable_extra_data = enable_extra_data

        self._wireframe_prog = self._load_prog('wireframe.vert', 'wireframe.frag', 'wireframe2.geom')
        self._shadow_prog = self._load_prog('shadow.vert', 'shadow.frag')
        if self._enable_extra_data:
            self._extra_data_prog = self._load_prog('extra_data.vert', 'extra_data.frag')
        self._prog = self._load_prog('shader_v400.vert', 'shader_v400.frag')

        self._cbo = self._ctx.renderbuffer((view_width, view_height), samples=antialias_samples, dtype='f4')
        self._dbo = self._ctx.depth_texture((view_width, view_height), samples=antialias_samples, alignment=1)
        self._fbo = self._ctx.framebuffer([self._cbo], self._dbo)

        if self._samples > 0:
            self._cbo2 = self._ctx.renderbuffer((view_width, view_height), dtype='f4')
            self._dbo2 = self._ctx.depth_texture((view_width, view_height), alignment=1)
            self._fbo2 = self._ctx.framebuffer([self._cbo2], self._dbo2)

        # for shadows
        n = int(math.sqrt(self._samples or 1))
        self._scbo = self._ctx.renderbuffer((view_width*n, view_height*n))
        self._sdbo = self._ctx.depth_texture((view_width*n, view_height*n), alignment=1)
        self._sfbo = self._ctx.framebuffer([self._scbo], self._sdbo)

        self._vbos = []
        self._objs = []
        self._s_vbos = []
        self._s_objs = []
        self._e_vbos = []
        self._e_objs = []
        self._w_vbos = []
        self._w_objs = []
        self._raw_objs = []
        self._textures = []
        self._proj_mx = None
        self._view_mx = np.identity(4)
        self._model_mx = None
        self._frustum_near = None
        self._frustum_far = None
        self._persp_proj = False

    def __del__(self):
        self._wireframe_prog.release()
        self._shadow_prog.release()
        if self._enable_extra_data:
            self._extra_data_prog.release()
        self._prog.release()
        self._cbo.release()
        self._dbo.release()
        self._fbo.release()
        self._scbo.release()
        self._sdbo.release()
        self._sfbo.release()
        if self._samples > 0:
            self._cbo2.release()
            self._dbo2.release()
            self._fbo2.release()
        for o in self._objs:
            o.release()
        for o in self._vbos:
            o.release()
        for o in self._s_objs:
            o.release()
        for o in self._s_vbos:
            o.release()
        for o in self._e_objs:
            o.release()
        for o in self._e_vbos:
            o.release()
        for o in self._w_objs:
            o.release()
        for o in self._w_vbos:
            o.release()
        for t in self._textures:
            if t is not None:
                t.release()

    def _load_prog(self, vert, frag, geom=None):
        vertex_shader_source = open(os.path.join(os.path.dirname(__file__), vert)).read()
        fragment_shader_source = open(os.path.join(os.path.dirname(__file__), frag)).read()
        geom_shader_source = None if geom is None else open(os.path.join(os.path.dirname(__file__), geom)).read()
        return self._ctx.program(vertex_shader=vertex_shader_source,
                                 fragment_shader=fragment_shader_source,
                                 geometry_shader=geom_shader_source)

    @property
    def ctx(self):
        return self._ctx

    @property
    def width(self):
        return self._width

    @property
    def height(self):
        return self._height

    def set_frustum(self, x_fov, y_fov, frustum_near, frustum_far):
        self._frustum_near = frustum_near
        self._frustum_far = frustum_far
        self._persp_proj = True

        # calculate projection matrix based on frustum
        n = frustum_near
        f = frustum_far
        r = n * math.tan(math.radians(x_fov/2))
        t = n * math.tan(math.radians(y_fov/2))

        self._proj_mx = np.zeros((4, 4))
        self._proj_mx[0, 0] = n/r
        self._proj_mx[1, 1] = n/t
        self._proj_mx[2, 2] = -(f+n)/(f-n)
        self._proj_mx[3, 2] = -1
        self._proj_mx[2, 3] = -2*f*n/(f-n)

    @property
    def frustum_near(self):
        return self._frustum_near

    @property
    def frustum_far(self):
        return self._frustum_far

    def set_orth_frustum(self, width, height, frustum_near, frustum_far):
        self._frustum_near = n = frustum_near
        self._frustum_far = f = frustum_far
        self._persp_proj = False

        l = -width/2
        r = width/2
        b = -height/2
        t = height/2
        self._proj_mx = self._ortho_mx_size(l, r, b, t, n, f)

    def load_object(self, object, obj_idx=None, smooth=False, wireframe=False, cache_file=None):
        if cache_file is None or not os.path.isfile(cache_file):
            if isinstance(object, str):
                object = ShapeModel(fname=object)
            elif isinstance(object, Obj):
                object = ShapeModel(data={
                    'faces': np.array(object.face, dtype=np.uint32)[:, ],
                    'vertices': np.array(object.vert, dtype=np.float32),
                    'normals': np.array(object.norm, dtype=np.float32),
                    'texcoords': np.array(object.text, dtype=np.float32)[:, :2],
                })
            object.load_texture()
            obj_bytes = None if wireframe else object.pack_all()
            s_obj_bytes = object.pack_simple()
            object = None if wireframe else object

            if cache_file is not None:
                with open(cache_file, 'wb') as fh:
                    pickle.dump((object, obj_bytes, s_obj_bytes), fh)
        else:
            with open(cache_file, 'rb') as fh:
                object, obj_bytes, s_obj_bytes = pickle.load(fh)

        if wireframe:
            return self.load_cached_wf_object(s_obj_bytes, obj_idx=obj_idx)
        else:
            return self.load_cached_object(object, obj_bytes, s_obj_bytes, obj_idx=obj_idx)

    def load_cached_object(self, object, obj_bytes, s_obj_bytes, obj_idx=None):
        if obj_idx is not None:
            self._objs[obj_idx].release()
            self._vbos[obj_idx].release()
            self._s_objs[obj_idx].release()
            self._s_vbos[obj_idx].release()
            if self._enable_extra_data:
                self._e_objs[obj_idx].release()
                self._e_vbos[obj_idx].release()
            self._textures[obj_idx].release()

        texture = None
        if object.tex is not None:
            texture = self._ctx.texture(object.tex.T.shape, 1, np.flipud(object.tex).tobytes(), dtype='f4')
            texture.build_mipmaps()

        # TODO: check if this can be done and if it's faster:
        # if obj_idx is not None:
        #   self._vbos[obj_idx].orphan(len(obj_bytes))
        #   self._vbos[obj_idx].write(obj_bytes)
        #   # no changes to obj
        # else:
        #   # continue as before

        vbo = self._ctx.buffer(obj_bytes)
        obj = self._ctx.simple_vertex_array(self._prog, vbo, 'vertexPosition_modelFrame', 'vertexNormal_modelFrame', 'aTexCoords')

        s_vbo = self._ctx.buffer(s_obj_bytes)
        s_obj = self._ctx.simple_vertex_array(self._shadow_prog, s_vbo, 'vertexPosition_modelFrame')

        if self._enable_extra_data:
            e_vbo = self._ctx.buffer(obj_bytes)
            e_obj = self._ctx.simple_vertex_array(self._extra_data_prog, e_vbo, 'vertexPosition_modelFrame',
                                                  'vertexNormal_modelFrame', 'aTexCoords')

        if obj_idx is None:
            self._vbos.append(vbo)
            self._objs.append(obj)
            self._s_vbos.append(s_vbo)
            self._s_objs.append(s_obj)
            if self._enable_extra_data:
                self._e_vbos.append(e_vbo)
                self._e_objs.append(e_obj)
            self._textures.append(texture)
            self._raw_objs.append(object)
        else:
            self._vbos[obj_idx] = vbo
            self._objs[obj_idx] = obj
            self._s_vbos[obj_idx] = s_vbo
            self._s_objs[obj_idx] = s_obj
            if self._enable_extra_data:
                self._e_vbos[obj_idx] = e_vbo
                self._e_objs[obj_idx] = e_obj
            self._raw_objs[obj_idx] = object
            self._textures[obj_idx] = texture

        return len(self._objs) - 1

    def load_cached_wf_object(self, w_obj_bytes, obj_idx=None):
        if obj_idx is not None:
            self._w_objs[obj_idx].release()
            self._w_vbos[obj_idx].release()

        w_vbo = self._ctx.buffer(w_obj_bytes)
        w_obj = self._ctx.simple_vertex_array(self._wireframe_prog, w_vbo, 'vertexPosition_modelFrame')
        if obj_idx is None:
            self._w_vbos.append(w_vbo)
            self._w_objs.append(w_obj)
        else:
            self._w_vbos[obj_idx] = w_vbo
            self._w_objs[obj_idx] = w_obj
        return len(self._w_objs) - 1

    def ray_intersect_dist(self, obj_idxs, rel_pos_v, rel_rot_q):
        # return distance to objects along -z-axis, supports laser algorithm, put here because efficient

        if False:
            # Should find the nearest intersection with object faces on the camera axis.
            # However, tools.intersections return some error code, seems difficult to debug..

            candidates = []
            ray = np.array([0, 0, -1.0]).reshape((3, 1))
            for i, obj_idx in enumerate(obj_idxs):
                verts = tools.q_times_mx(rel_rot_q[i], self._raw_objs[obj_idx].vertices) + rel_pos_v[i]
                x = tools.intersections(self._raw_objs[obj_idx].faces, verts, ray)
                candidates.extend(np.abs(x))
            dist = np.min(candidates) if len(candidates)>0 else None
        else:
            # alternative method: just render and pick center pixel
            _, depth = self.render(obj_idxs, rel_pos_v, rel_rot_q, [1, 0, 0],
                                   get_depth=True, shadows=False, textures=False)
            dist = depth[depth.shape[0]//2, depth.shape[1]//2]
            if dist >= self._frustum_far * 0.99:
                dist = None

        return dist

    def render_wireframe(self, obj_idxs, rel_pos_v, rel_rot_q, color):
        obj_idxs = [obj_idxs] if isinstance(obj_idxs, int) else obj_idxs
        rel_pos_v = np.array(rel_pos_v).reshape((-1, 3))
        rel_rot_q = np.array(rel_rot_q).reshape((-1,))
        color = np.array(color).reshape((-1, 3))
        assert len(obj_idxs) == rel_pos_v.shape[0] == rel_rot_q.shape[0], 'obj_idxs, rel_pos_v and rel_rot_q dimensions dont match'

        self._fbo.use()
        self._ctx.disable(moderngl.DEPTH_TEST)
        self._ctx.disable(moderngl.CULL_FACE)
        #self._ctx.front_face = 'ccw'  # cull back faces (front faces suggested but that had glitches)
        self._ctx.clear(0, 0, 0, float('inf'))

        # self._ctx.clear(depth=float('inf'))
        for i, obj_idx in enumerate(obj_idxs):
            self._set_params(obj_idx, rel_pos_v[i], rel_rot_q[i], self._wireframe_prog)
            self._wireframe_prog['color'].value = tuple(color[i])
            self._w_objs[obj_idx].render()

        if self._samples > 0:
            self._ctx.copy_framebuffer(self._fbo2, self._fbo)
            fbo = self._fbo2
        else:
            fbo = self._fbo

        data = np.frombuffer(fbo.read(components=3, alignment=1), dtype='u1').reshape((self._height, self._width, 3))
        data = np.flipud(data)

        return data

    def render_extra_data(self, obj_idxs, rel_pos_v, rel_rot_q, light_v):
        assert self._enable_extra_data, 'RenderEngine constructed with enable_extra_data=False'

        obj_idxs = [obj_idxs] if isinstance(obj_idxs, int) else obj_idxs
        rel_pos_v = np.array(rel_pos_v).reshape((-1, 3))
        rel_rot_q = np.array(rel_rot_q).reshape((-1,))
        light_v = np.array(light_v)
        assert len(obj_idxs) == rel_pos_v.shape[0] == rel_rot_q.shape[0], 'obj_idxs, rel_pos_v and rel_rot_q dimensions dont match'

        self._fbo.use()
        self._ctx.enable(moderngl.DEPTH_TEST)
        self._ctx.enable(moderngl.CULL_FACE)
        self._ctx.front_face = 'ccw'  # cull back faces
        self._ctx.clear(np.nan, np.nan, np.nan, np.nan, float('inf'))

        for i, obj_idx in enumerate(obj_idxs):
            self._set_params(obj_idx, rel_pos_v[i], rel_rot_q[i], light_v, prog=self._extra_data_prog)
            self._extra_data_prog['select'].value = 0  # 0: px model coords, 1: light incidence, emission, phase angle (cos)
            self._e_objs[obj_idx].render()

        if self._samples > 0:
            self._ctx.copy_framebuffer(self._fbo2, self._fbo)
            fbo = self._fbo2
        else:
            fbo = self._fbo

        data = np.frombuffer(fbo.read(components=3, alignment=1, dtype='f4'), dtype='f4')\
                 .reshape((self._height, self._width, 3))
        data = np.flipud(data)

        return data

    # @profile(stream=open('memory_profiler.log', 'w+'))
    def render(self, obj_idxs, rel_pos_v, rel_rot_q, light_v, get_depth=False, shadows=True, textures=True,
               gamma=1.0, reflection=REFLMOD_LUNAR_LAMBERT, flux_density=False):

        obj_idxs = [obj_idxs] if isinstance(obj_idxs, int) else obj_idxs
        rel_pos_v = np.array(rel_pos_v).reshape((-1, 3))
        rel_rot_q = np.array(rel_rot_q).reshape((-1,))
        light_v = np.array(light_v)
        assert len(obj_idxs) == rel_pos_v.shape[0] == rel_rot_q.shape[0], 'obj_idxs, rel_pos_v and rel_rot_q dimensions dont match'

        shadow_mvps = None
        if shadows:
            shadow_mvps = self._render_shadowmap(obj_idxs, rel_pos_v, rel_rot_q, light_v)

        self._fbo.use()
        self._ctx.enable(moderngl.DEPTH_TEST)
        self._ctx.enable(moderngl.CULL_FACE)
        self._ctx.front_face = 'ccw'  # cull back faces
        self._ctx.clear(0, 0, 0, 0, float('inf'))
        if shadows:
            self._shadow_map.use(RenderEngine._LOC_SHADOW_MAP)
            self._prog['shadow_map'].value = RenderEngine._LOC_SHADOW_MAP

        for i, obj_idx in enumerate(obj_idxs):
            self._set_params(obj_idx, rel_pos_v[i], rel_rot_q[i], light_v, shadow_mvps,
                             textures, reflection, prog=self._prog, flux_density=flux_density)
            self._objs[obj_idx].render()

        if self._samples > 0:
            self._ctx.copy_framebuffer(self._fbo2, self._fbo)
            fbo = self._fbo2
            dbo = self._dbo2
        else:
            fbo = self._fbo
            dbo = self._dbo

        data = np.frombuffer(fbo.read(components=1, alignment=1, dtype='f4'), dtype='f4').reshape((self._height, self._width))
        data = np.flipud(data)

        if get_depth:
            depth = np.frombuffer(dbo.read(alignment=1), dtype='f4').reshape((self._height, self._width))
            depth = np.flipud(depth)

            # normalize depth
            if self._persp_proj:
                # for perspective projection
                a = -(self._frustum_far - self._frustum_near) / (2.0 * self._frustum_far * self._frustum_near)
                b = (self._frustum_far + self._frustum_near) / (2.0 * self._frustum_far * self._frustum_near)
                if self._frustum_far/self._frustum_near < 1e7:
                    depth = np.divide(1.0, (2.0 * a) * depth - (a - b))  # 1/((2*X-1)*a+b)
                else:
                    # up to difference of 1e14
                    depth = np.divide(1.0, (2.0 * a) * depth.astype(np.float64) - (a - b)).astype(np.float32)
            else:
                # for orthographic projection
                #  - depth is between 0 and 1
                depth = depth * (self._frustum_far - self._frustum_near) + self._frustum_near

        # free memory to avoid memory leaks
        if shadows:
            self._shadow_map.release()

        if flux_density:
            data = data * flux_density
        else:
            data = np.clip(data*255, 0, 255).astype('uint8')
            if gamma != 1.0:
                data = ImageProc.adjust_gamma(data, gamma)

        return (data, depth) if get_depth else data

    def _set_params(self, obj_idx, rel_pos_v, rel_rot_q, light_v=None, shadow_mvps=None, use_textures=True,
                    reflection=REFLMOD_LUNAR_LAMBERT, prog=None, flux_density=False):

        self._model_mx = np.identity(4)
        self._model_mx[:3, :3] = quaternion.as_rotation_matrix(rel_rot_q)
        self._model_mx[:3, 3] = rel_pos_v

        prog = prog or self._prog
        mv = self._view_mx.dot(self._model_mx)
        mvp = self._proj_mx.dot(mv)
        prog['mvp'].write((mvp.T).astype('float32').tobytes())

        if 'mv' in prog:
            prog['mv'].write((mv.T).astype('float32').tobytes())

        if 'use_texture' in prog:
            use_texture = use_textures and self._textures[obj_idx] is not None
            prog['use_texture'].value = use_texture
            if use_texture:
                self._textures[obj_idx].use(RenderEngine._LOC_TEXTURE)
                prog['texture_map'].value = RenderEngine._LOC_TEXTURE

        if 'reflection_model' in prog:
            prog['use_flux_density'].value = flux_density is not False
            prog['brightness_coef'].value = 1 if flux_density else 0.65
            prog['reflection_model'].value = reflection
            prog['model_coefs'].value = RenderEngine.REFLMOD_PARAMS[reflection]

        if 'lightDirection_viewFrame' in prog:
            prog['lightDirection_viewFrame'].value = tuple(-light_v)  # already in view frame

        if 'use_shadows' in prog:
            use_shadows = shadow_mvps is not None
            prog['use_shadows'].value = use_shadows
            if use_shadows:
                self._prog['shadow_mvp'].write(shadow_mvps[obj_idx].T.astype('float32').tobytes())

        if 'reflection_model' in prog:
            if reflection == RenderEngine.REFLMOD_HAPKE and RenderEngine.REFLMOD_PARAMS[reflection][9] % 2 > 0:
                hapke_K = self._ctx.texture((7, 19), 1, data=RenderEngine.HAPKE_K.T.astype('float32').tobytes(), alignment=1, dtype='f4')
                hapke_K.use(RenderEngine._LOC_HAPKE_K)
                prog['hapke_K'].value = RenderEngine._LOC_HAPKE_K

    def _render_shadowmap(self, obj_idxs, rel_pos_v, rel_rot_q, light_v):
        # shadows following http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-16-shadow-mapping/

        v = np.identity(4)
        angle = math.acos(np.clip(np.array([0, 0, -1]).dot(light_v), -1, 1))
        axis = np.cross(np.array([0, 0, -1]), light_v)
        q_cam2light = tools.angleaxis_to_q((angle, *axis))
        v[:3, :3] = quaternion.as_rotation_matrix(q_cam2light.conj())

        mvs = {}
        for i, obj_idx in enumerate(obj_idxs):
            m = np.identity(4)
            m[:3, :3] = quaternion.as_rotation_matrix(rel_rot_q[obj_idx])
            m[:3, 3] = rel_pos_v[i]
            mv = v.dot(m)
            mvs[obj_idx] = mv

        proj = self._ortho_mx(obj_idxs, mvs)
        bias = self._bias_mx()  # map from [-1,1] x [-1,1] to [0,1]x[0,1] so that can use with "texture" command

        self._sfbo.use()
        self._ctx.enable(moderngl.DEPTH_TEST)
        self._ctx.enable(moderngl.CULL_FACE)
        self._ctx.front_face = 'ccw'  # cull back faces (front faces suggested but that had glitches)

        self._ctx.clear(depth=float('inf'))
        shadow_mvps = {}
        for i in obj_idxs:
            mvp = proj.dot(mvs[i])
            shadow_mvps[i] = bias.dot(mvp)  # used later to project model vertices to same 2d shadow frame
            self._shadow_prog['mvp'].write(mvp.T.astype('float32').tobytes())
            self._s_objs[i].render()

        data = self._sdbo.read(alignment=1)
        n = int(math.sqrt(self._samples or 1))
        self._shadow_map = self._ctx.texture((self._width*n, self._height*n), 1, data=data, alignment=1, dtype='f4')

        if False:
            import cv2
            d = np.frombuffer(data, dtype='f4').reshape((self._width, self._height))
            a = np.max(d.flatten())
            b = np.min(d.flatten())
            print('%s,%s' % (a, b))
            cv2.imshow('distance from sun', d)
            #cv2.waitKey()
            #quit()

        return shadow_mvps

    def _ortho_mx_size(self, l, r, b, t, n, f):
        P = np.identity(4)
        P[0, 0] = 2 / (r - l)
        P[1, 1] = 2 / (t - b)
        P[2, 2] = -2 / (f - n)
        P[0, 3] = -(r + l) / (r - l)
        P[1, 3] = -(t + b) / (t - b)
        P[2, 3] = -(f + n) / (f - n)

        if False:
            # transform should result that all are in range [-1,1]
            tr = P.dot(np.array([
                [l, b, n, 1],
                [r, t, f, 1],
            ]).T)
            print('%s'%tr)

        return P

    def _ortho_mx(self, obj_idxs, mvs):
        l = float('inf') # min x
        r = -float('inf') # max x
        b = float('inf') # min y
        t = -float('inf') # max y
        n = float('inf') # min z
        f = -float('inf') # max z
        for i, obj_idx in enumerate(obj_idxs):
            v3d = self._raw_objs[obj_idx].vertices
            vert = mvs[obj_idx].dot(np.concatenate((v3d, np.ones((len(v3d),1))), axis=1).T).T
            vert = vert[:, :3] / vert[:, 3:]
            x0, y0, z0 = np.min(vert, axis=0)
            x1, y1, z1 = np.max(vert, axis=0)
            l = min(l, x0)
            r = max(r, x1)
            b = min(b, y0)
            t = max(t, y1)
            n = min(n, -z1)  # negative z-axis in front of camera, however, near and far values typically positive
            f = max(f, -z0)

        P = self._ortho_mx_size(l, r, b, t, n, f)

        return P

    def _bias_mx(self):
        return np.array([
            [0.5, 0.0, 0.0, 0.5],
            [0.0, 0.5, 0.0, 0.5],
            [0.0, 0.0, 0.5, 0.5],
            [0.0, 0.0, 0.0, 1.0],
        ]) class PositioningException(Exception):
class Stopwatch:
class NearestKernelNDInterpolator(NearestNDInterpolator):
    def __init__(self, elapsed=0.0, func=time.perf_counter):
    def elapsed(self):
    def start(self):
    def stop(self):
    def reset(self):
    def running(self):
    def __enter__(self):
    def __exit__(self, *args):
def sphere_angle_radius(loc, r):
def dist_across_and_along_vect(A, b):
def point_vector_dist(A, B, dist_along_v=False):
def sc_asteroid_max_shift_error(A, B):
def cross3d(left, right):
def normalize_v(v):
def normalize_v_f8(v):
def generate_field_fft(shape, sd=(0.33, 0.33, 0.34), len_sc=(0.5, 0.5 / 4, 0.5 / 16)):
def _surf_normal(x1, x2, x3):
def surf_normal(x1, x2, x3):
def vector_projection(a, b):
def vector_rejection(a, b):
def angle_between_v(v1, v2):
def angle_between_v_mx(a, B, normalize=True):
def angle_between_mx(A, B):
def angle_between_rows(A, B, normalize=True):
def rand_q(angle):
def angle_between_q(q1, q2):
def angle_between_q_arr(q1, q2):
def angle_between_ypr(ypr1, ypr2):
def distance_mx(A, B):
def q_to_unitbase(q):
def equatorial_to_ecliptic(ra, dec):
def q_to_angleaxis(q, compact=False):
def angleaxis_to_q(rv):
def ypr_to_q(lat, lon, roll):
def eul_to_q(angles, order='xyz', reverse=False):
def q_to_ypr(q):
def mean_q(qs, ws=None):
def q_times_v(q, v):
def q_times_mx(q, mx):
def mx2qmx(mx):
def wrap_rads(a):
def wrap_degs(a):
def eccentric_anomaly(eccentricity, mean_anomaly, tol=1e-6):
def solar_elongation(ast_v, sc_q):
def find_nearest_lesser(array, value):
def find_nearest_greater(array, value):
def find_nearest(array, value):
def find_nearest_arr(array, value, ord=None, fun=None):
def find_nearest_n(array, value, r, ord=None, fun=None):
def find_nearest_each(haystack, needles, ord=None):
def cartesian2spherical(x, y, z):
def spherical2cartesian(lat, lon, r):
def spherical2cartesian_arr(A, r=None):
def discretize_v(v, tol=None, lat_range=(-math.pi / 2, math.pi / 2), points=None):
def discretize_q(q, tol=None, lat_range=(-math.pi / 2, math.pi / 2), points=None):
def bf_lat_lon(tol, lat_range=(-math.pi / 2, math.pi / 2)):
def bf2_lat_lon(tol, lat_range=(-math.pi / 2, math.pi / 2)):
def robust_mean(arr, discard_percentile=0.2, ret_n=False, axis=None):
def robust_std(arr, discard_percentile=0.2, mean=None, axis=None):
def mv_normal(mean, cov=None, L=None, size=None):
def point_cloud_vs_model_err(points: np.ndarray, model) -> np.ndarray:
def poly_line_intersect(poly, line):
def intersections(faces, vertices, line):
def get_model_errors(points, vertices, faces):
def crop_model(model, cam_v, cam_q, x_fov, y_fov):
def augment_model(model, multiplier=3, length_scales=(0, 0.1, 1), sds=(1e-5, 1.6e-4, 2.4e-4)):
def apply_noise(model, support=(None, None), L=(None, None), len_sc=SHAPE_MODEL_NOISE_LEN_SC,
                noise_lv=SHAPE_MODEL_NOISE_LV['lo'], only_z=False,
                tx_noise=0, tx_noise_len_sc=SHAPE_MODEL_NOISE_LEN_SC, tx_hf_noise=True):
def texture_noise(model, support=None, L=None, noise_sd=SHAPE_MODEL_NOISE_LV['lo'],
                  len_sc=SHAPE_MODEL_NOISE_LEN_SC, max_rng=None, max_n=1e4, hf_noise=True):
    def __init__(self, *args, k_nearest=None, kernel='gaussian', kernel_sc=None,
                 kernel_eps=1e-12, query_eps=0.05, max_distance=None, **kwargs):
    def _linear(self, r):
    def _quadratic(self, r):
    def _cubic(self, r):
    def _gaussian(self, r):
    def __call__(self, *args):
def points_with_noise(points, support=None, L=None, noise_lv=SHAPE_MODEL_NOISE_LV['lo'],
                      len_sc=SHAPE_MODEL_NOISE_LEN_SC, max_rng=None, only_z=False):
def foreground_idxs(array, max_val=None):
def interp2(array, x, y, max_val=None, max_dist=30, idxs=None, discard_bg=False):
def solve_rotation(src_q, dst_q):
def solve_q_bf(src_q, dst_q):
def hover_annotate(fig, ax, line, annotations):
    def update_annot(ind):
    def hover(event):
def plot_vectors(pts3d, scatter=True, conseq=True, neg_z=True):
def numeric(s):
def pseudo_huber_loss(a, delta):
def fixed_precision(val, precision, as_str=False):
def plot_quats(quats, conseq=True, wait=True):
def plot_poses(poses, conseq=True, wait=True, arrow_len=1):
def display_top(top_stats, key_type='lineno', limit=10):
def show_progress(tot, i):
def smooth1d(xt, x, Y, weight_fun=lambda d: 0.9 ** abs(d)):
    R = quaternion.as_rotation_matrix(q)
    A_B = (diagAB / normB2) * B
    AB2A = A - A_B
            A = A / np.linalg.norm(A, axis=1).reshape((-1, 1))
            B = B / np.linalg.norm(B, axis=1).reshape((-1, 1))
    O = np.repeat(A.reshape((-1, 1, k)), B.shape[0], axis=1) - np.repeat(B.reshape((1, -1, k)), A.shape[0], axis=0)
    D = np.linalg.norm(O, axis=2)
    U0 = quaternion.as_quat_array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1.]])
    E = mean_anomaly if eccentricity < 0.8 else math.pi
    F = E - eccentricity * math.sin(mean_anomaly) - mean_anomaly;
        E = E - F / (1.0 - eccentricity * math.cos(E))
        F = E - eccentricity * math.sin(E) - mean_anomaly
    I = np.where(array < value)
    I = np.where(array > value)
    J = np.logical_not(np.isnan(arr))
        J = np.all(J, axis=1 if axis == 0 else 0)
    I = np.logical_and(low < arr, arr < high)
        I = np.all(I, axis=1 if axis == 0 else 0)
        L = np.linalg.cholesky(cov)
    M = np.zeros((4, 4))
    D = np.repeat(np.expand_dims(xt, 1), len(x), axis=1) - np.repeat(np.expand_dims(x, 0), len(xt), axis=0) class SystemModel(ABC):
    (
        OPENGL_FRAME,
        SPACECRAFT_FRAME,
        ASTEROID_FRAME,
        OPENCV_FRAME,
    ) = range(4)

    # from sc cam frame (axis: +x, up: +z) to opengl (axis -z, up: +y)
    sc2gl_q = np.quaternion(0.5, 0.5, -0.5, -0.5)

    # from opencv cam frame (axis: +z, up: -y) to opengl (axis -z, up: +y)
    cv2gl_q = np.quaternion(0, 1, 0, 0)

    def __init__(self, asteroid, camera, limits, *args, **kwargs):
        super(SystemModel, self).__init__()

        self.asteroid = asteroid
        self.cam = camera

        # mission limits
        (
            self.min_distance,      # min_distance in km
            self.min_med_distance,  # min_med_distance in km
            self.max_med_distance,  # max_med_distance in km
            self.max_distance,      # max_distance in km
            self.min_elong,         # min_elong in deg
            self.min_time           # min time instant as astropy.time.Time
        ) = limits

        assert self.min_altitude > 0, \
            'min distance %.2fkm too small, possible collision as asteroid max_radius=%.0fm' % (
            self.min_distance, self.asteroid.max_radius)

        self.mission_id = None  # overridden by particular missions
        self.view_width = VIEW_WIDTH

        # spacecraft position relative to asteroid, z towards spacecraft,
        #   x towards right when looking out from s/c camera, y up
        self.x_off = Parameter(-4, 4, estimate=False)
        self.y_off = Parameter(-4, 4, estimate=False)

        # whole view: 1.65km/tan(2.5deg) = 38km
        # can span ~30px: 1.65km/tan(2.5deg * 30/1024) = 1290km
        self.z_off = Parameter(-self.max_distance, -self.min_distance, def_val=-self.min_med_distance,
                               is_gl_z=True)  # was 120, 220

        # spacecraft orientation relative to stars
        self.x_rot = Parameter(-90, 90, estimate=False)  # axis latitude
        self.y_rot = Parameter(-180, 180, estimate=False)  # axis longitude
        self.z_rot = Parameter(-180, 180, estimate=False)  # rotation

        # asteroid zero orientation relative to stars
        self.ast_x_rot = Parameter(-90, 90, estimate=False)  # axis latitude
        self.ast_y_rot = Parameter(-180, 180, estimate=False)  # axis longitude
        self.ast_z_rot = Parameter(-180, 180, estimate=False)  # rotation
        self.asteroid_rotation_from_model()

        # time in seconds since 1970-01-01 00:00:00
        self.time = Parameter(
            self.min_time.unix,
            self.min_time.unix + self.asteroid.rotation_period,
            estimate=False
        )

        # history of states accumulated by using propagate()
        self.state_history = []

        # override any default params
        for n, v in kwargs.items():
            setattr(self, n, v)

        # set default values to params
        for n, p in self.get_params():
            p.value = p.def_val

    @property
    def min_altitude(self):
        """ in km """
        return self.min_distance - self.asteroid.max_radius / 1000

    @property
    def view_height(self):
        return int(self.cam.height * self.view_width / self.cam.width)

    def get_params(self, all=False):
        return (
            (n, getattr(self, n))
            for n in sorted(self.__dict__)
            if isinstance(getattr(self, n), Parameter)
               and (all or getattr(self, n).estimate)
        )

    def param_change_events(self, enabled):
        for n, p in self.get_params(all=True):
            p.fire_change_events = enabled

    @property
    def spacecraft_pos(self):
        return self.x_off.value, self.y_off.value, self.z_off.value

    @spacecraft_pos.setter
    def spacecraft_pos(self, pos):
        self.z_off.value = pos[2]

        half_range = abs(pos[2] / 170 * 4)
        self.x_off.range = (pos[0] - half_range, pos[0] + half_range)
        self.x_off.value = pos[0]

        self.y_off.range = (pos[1] - half_range, pos[1] + half_range)
        self.y_off.value = pos[1]

    @property
    def spacecraft_rot(self):
        return self.x_rot.value, self.y_rot.value, self.z_rot.value

    @spacecraft_rot.setter
    def spacecraft_rot(self, r):
        self.x_rot.value, self.y_rot.value, self.z_rot.value = r

    @property
    def asteroid_axis(self):
        return self.ast_x_rot.value, self.ast_y_rot.value, self.ast_z_rot.value

    @asteroid_axis.setter
    def asteroid_axis(self, r):
        self.ast_x_rot.value, self.ast_y_rot.value, self.ast_z_rot.value = r
        self.update_asteroid_model()

    @property
    def spacecraft_dist(self):
        return math.sqrt(sum(x ** 2 for x in self.spacecraft_pos))

    @property
    def spacecraft_altitude(self):
        sc_ast_v = tools.normalize_v(np.array(self.spacecraft_pos))
        ast_vx = self.sc_asteroid_vertices()
        min_distance = np.min(sc_ast_v.dot(ast_vx.T))
        return min_distance

    @property
    def real_spacecraft_altitude(self):
        sc_ast_v = tools.normalize_v(np.array(self.real_spacecraft_pos))
        ast_vx = self.sc_asteroid_vertices(real=True)
        min_distance = np.min(sc_ast_v.dot(ast_vx.T))
        return min_distance

    def asteroid_rotation_from_model(self):
        self.ast_x_rot.value = math.degrees(self.asteroid.axis_latitude)
        self.ast_y_rot.value = math.degrees(self.asteroid.axis_longitude)
        self.ast_z_rot.value = (math.degrees(self.asteroid.rotation_pm) + 180) % 360 - 180

    def update_asteroid_model(self):
        self.asteroid.axis_latitude = math.radians(self.ast_x_rot.value)
        self.asteroid.axis_longitude = math.radians(self.ast_y_rot.value)
        self.asteroid.rotation_pm = math.radians(self.ast_z_rot.value)

    def pixel_extent(self, distance=None):
        distance = abs(self.z_off) if distance is None else distance
        return self.cam.width * math.atan(self.asteroid.mean_radius / 1000 / distance) * 2 / math.radians(
            self.cam.x_fov)

    @property
    def real_spacecraft_pos(self):
        return self.x_off.real_value, self.y_off.real_value, self.z_off.real_value

    @real_spacecraft_pos.setter
    def real_spacecraft_pos(self, rv):
        self.x_off.real_value, self.y_off.real_value, self.z_off.real_value = rv

    @property
    def real_spacecraft_rot(self):
        return self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value

    @real_spacecraft_rot.setter
    def real_spacecraft_rot(self, rv):
        self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value = rv

    @property
    def real_asteroid_axis(self):
        return self.ast_x_rot.real_value, self.ast_y_rot.real_value, self.ast_z_rot.real_value

    @real_asteroid_axis.setter
    def real_asteroid_axis(self, rv):
        self.ast_x_rot.real_value, self.ast_y_rot.real_value, self.ast_z_rot.real_value = rv

    @property
    def spacecraft_q(self):
        return tools.ypr_to_q(*list(map(
            math.radians,
            (self.x_rot.value, self.y_rot.value, self.z_rot.value)
        )))

    @spacecraft_q.setter
    def spacecraft_q(self, new_q):
        self.x_rot.value, self.y_rot.value, self.z_rot.value = \
            list(map(math.degrees, tools.q_to_ypr(new_q)))

    @property
    def real_spacecraft_q(self):
        return tools.ypr_to_q(*list(map(
            math.radians,
            (self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value)
        )))

    @real_spacecraft_q.setter
    def real_spacecraft_q(self, new_q):
        self.x_rot.real_value, self.y_rot.real_value, self.z_rot.real_value = \
            list(map(math.degrees, tools.q_to_ypr(new_q)))

    @property
    def asteroid_q(self):
        return self.asteroid.rotation_q(self.time.value)

    @asteroid_q.setter
    def asteroid_q(self, new_q):
        ast = self.asteroid
        sc2ast_q = SystemModel.frm_conv_q(SystemModel.SPACECRAFT_FRAME, SystemModel.ASTEROID_FRAME, ast=ast)

        ast.axis_latitude, ast.axis_longitude, new_theta = tools.q_to_ypr(new_q * sc2ast_q)

        old_theta = ast.rotation_theta(self.time.value)
        ast.rotation_pm = tools.wrap_rads(ast.rotation_pm + new_theta - old_theta)

        self.asteroid_rotation_from_model()

    @property
    def real_asteroid_q(self):
        org_ast_axis = self.asteroid_axis
        self.asteroid_axis = self.real_asteroid_axis
        q = self.asteroid.rotation_q(self.time.real_value)
        self.asteroid_axis = org_ast_axis
        return q

    @real_asteroid_q.setter
    def real_asteroid_q(self, new_q):
        org_ast_axis = self.asteroid_axis
        self.asteroid_axis = self.real_asteroid_axis
        self.asteroid_q = new_q
        self.asteroid_axis = org_ast_axis

    def gl_sc_asteroid_rel_q(self, discretize_tol=False):
        """ rotation of asteroid relative to spacecraft in opengl coords """
        assert not discretize_tol, 'discretize_tol deprecated at gl_sc_asteroid_rel_q function'
        self.update_asteroid_model()
        sc_ast_rel_q = SystemModel.sc2gl_q.conj() * self.sc_asteroid_rel_q()

        if discretize_tol:
            qq, _ = tools.discretize_q(sc_ast_rel_q, discretize_tol)
            err_q = sc_ast_rel_q * qq.conj()
            sc_ast_rel_q = qq

        if not BATCH_MODE and DEBUG:
            print('asteroid x-axis: %s' % tools.q_times_v(sc_ast_rel_q, np.array([1, 0, 0])))

        return sc_ast_rel_q, err_q if discretize_tol else False

    def sc_asteroid_rel_q(self, time=None):
        """ rotation of asteroid relative to spacecraft in spacecraft coords """
        ast_q = self.asteroid.rotation_q(time or self.time.value)
        sc_q = self.spacecraft_q
        return sc_q.conj() * ast_q

    def real_sc_asteroid_rel_q(self):
        org_sc_rot = self.spacecraft_rot
        org_ast_axis = self.asteroid_axis
        self.spacecraft_rot = self.real_spacecraft_rot
        self.asteroid_axis = self.real_asteroid_axis

        q_tot = self.sc_asteroid_rel_q(time=self.time.real_value)

        self.spacecraft_rot = org_sc_rot
        self.asteroid_axis = org_ast_axis
        return q_tot

    def rotate_spacecraft(self, q):
        new_q = self.spacecraft_q * q
        self.x_rot.value, self.y_rot.value, self.z_rot.value = \
            list(map(math.degrees, tools.q_to_ypr(new_q)))

    def rotate_asteroid(self, q):
        """ rotate asteroid in spacecraft frame """
        # global rotation q on asteroid in sc frame, followed by local rotation to asteroid frame
        new_q = q * self.asteroid.rotation_q(self.time.value)
        self.asteroid_q = new_q

    def rotate_system(self, q):
        self.asteroid.real_position = tools.q_times_v(q, self.asteroid.position(self.time.value))
        self.rotate_asteroid(q)
        self.rotate_spacecraft(q)

    def get_vals(self, real=False):
        return {n: (p.real_value if real else p.value) for n, p in self.get_params(True)}

    def set_vals(self, vals, real=False):
        for n, v in vals.items():
            setattr(getattr(self, n), 'real_value' if real else 'value', v)

    def reset_to_real_vals(self):
        for n, p in self.get_params(True):
            assert p.real_value is not None, 'real value missing for %s' % n
            p.value = p.real_value

    def swap_values_with_real_vals(self):
        for n, p in self.get_params(True):
            assert p.real_value is not None, 'real value missing for %s' % n
            assert p.value is not None, 'current value missing %s' % n
            tmp = p.value
            p.value = p.real_value
            p.real_value = tmp

    def calc_shift_err(self):
        est_vertices = self.sc_asteroid_vertices()
        self.swap_values_with_real_vals()
        target_vertices = self.sc_asteroid_vertices()
        self.swap_values_with_real_vals()
        return tools.sc_asteroid_max_shift_error(est_vertices, target_vertices)

    def sc_asteroid_vertices(self, real=False):
        """ asteroid vertices rotated and translated to spacecraft frame """
        if self.asteroid.real_shape_model is None:
            return None

        sc_ast_q = self.real_sc_asteroid_rel_q() if real else self.sc_asteroid_rel_q()
        sc_pos = self.real_spacecraft_pos if real else self.spacecraft_pos

        return tools.q_times_mx(sc_ast_q, np.array(self.asteroid.real_shape_model.vertices)) + sc_pos

    def gl_light_rel_dir(self, err_q=False, discretize_tol=False):
        """ direction of light relative to spacecraft in opengl coords """
        assert not discretize_tol, 'discretize_tol deprecated at gl_light_rel_dir function'

        light_v, err_angle = self.light_rel_dir(err_q=False, discretize_tol=False)

        err_q = (err_q or np.quaternion(1, 0, 0, 0))
        light_gl_v = tools.q_times_v(err_q.conj() * SystemModel.sc2gl_q.conj(), light_v)

        # new way to discretize light, consistent with real fdb inplementation
        if discretize_tol:
            dlv, _ = tools.discretize_v(light_gl_v, discretize_tol,
                                        lat_range=(-math.pi / 2, math.radians(90 - self.min_elong)))
            err_angle = tools.angle_between_v(light_gl_v, dlv)
            light_gl_v = dlv

        return light_gl_v, err_angle

    def light_rel_dir(self, err_q=False, discretize_tol=False):
        """ direction of light relative to spacecraft in s/c coords """
        assert not discretize_tol, 'discretize_tol deprecated at light_rel_dir function'

        light_v = tools.normalize_v(self.asteroid.position(self.time.value))
        sc_q = self.spacecraft_q
        err_q = (err_q or np.quaternion(1, 0, 0, 0))

        # old, better way to discretize light, based on asteroid rotation axis, now not in use
        if discretize_tol:
            ast_q = self.asteroid_q
            light_ast_v = tools.q_times_v(ast_q.conj(), light_v)
            dlv, _ = tools.discretize_v(light_ast_v, discretize_tol)
            err_angle = tools.angle_between_v(light_ast_v, dlv)
            light_v = tools.q_times_v(ast_q, dlv)

        return tools.q_times_v(err_q.conj() * sc_q.conj(), light_v), \
               err_angle if discretize_tol else False

    def solar_elongation(self, real=False):
        ast_v = self.asteroid.position(self.time.real_value if real else self.time.value)
        sc_q = self.real_spacecraft_q if real else self.spacecraft_q
        elong, direc = tools.solar_elongation(ast_v, sc_q)
        if not BATCH_MODE and DEBUG:
            print('elong: %.3f | dir: %.3f' % (
                math.degrees(elong), math.degrees(direc)))
        return elong, direc

    def rel_rot_err(self):
        return tools.angle_between_q(
            self.sc_asteroid_rel_q(),
            self.real_sc_asteroid_rel_q())

    def lat_pos_err(self):
        real_pos = self.real_spacecraft_pos
        err = np.subtract(self.spacecraft_pos, real_pos)
        return math.sqrt(err[0] ** 2 + err[1] ** 2) / abs(real_pos[2])

    def dist_pos_err(self):
        real_d = self.real_spacecraft_pos[2]
        return abs(self.spacecraft_pos[2] - real_d) / abs(real_d)

    def calc_visibility(self, pos=None):
        if pos is None:
            pos = self.spacecraft_pos

        if isinstance(pos, np.ndarray):
            pos = pos.reshape((-1, 3))
            return_array = True
        else:
            pos = np.array([pos], shape=(1, 3))
            return_array = False

        rad = self.asteroid.mean_radius * 0.001
        xt = np.abs(pos[:, 2]) * math.tan(math.radians(self.cam.x_fov) / 2)
        yt = np.abs(pos[:, 2]) * math.tan(math.radians(self.cam.y_fov) / 2)

        # xm = np.clip((xt - (abs(pos[0])-rad))/rad/2, 0, 1)
        # ym = np.clip((yt - (abs(pos[1])-rad))/rad/2, 0, 1)
        xm = 1 - np.minimum(1, (np.maximum(0, pos[:, 0] + rad - xt) + np.maximum(0, rad - pos[:, 0] - xt)) / rad / 2)
        ym = 1 - np.minimum(1, (np.maximum(0, pos[:, 1] + rad - yt) + np.maximum(0, rad - pos[:, 1] - yt)) / rad / 2)
        visib = xm * ym * 100

        return visib if return_array else visib[0]

    def get_system_gf(self):
        # return all in ICRS frame
        sc_gf_q = self.spacecraft_q
        ast_gf_q = self.asteroid.rotation_q(self.time.value)
        sc_ast_gf_r = -tools.q_times_v(sc_gf_q, self.spacecraft_pos)
        sc_sun_gf_r = sc_ast_gf_r - self.asteroid.position(self.time.value)
        return sc_gf_q, ast_gf_q, sc_ast_gf_r, sc_sun_gf_r

    def get_system_scf(self):
        sc_ast_lf_r = tools.q_times_v(SystemModel.sc2gl_q, self.spacecraft_pos)
        sc_ast_lf_q = self.spacecraft_q.conj() * self.asteroid.rotation_q(self.time.value)
        ast_sun_lf_u = tools.q_times_v(self.spacecraft_q.conj(),
                                       -tools.normalize_v(self.asteroid.position(self.time.value)))
        return sc_ast_lf_r, sc_ast_lf_q, ast_sun_lf_u

    def get_cropped_system_scf(self, x, y, w, h):
        sc_ast_lf_r, sc_ast_lf_q, ast_sun_lf_u = self.get_system_scf()
        sc, dq = self.cropped_system_tf(x, y, w, h)

        # adjust position
        sc_ast_lf_r = tools.q_times_v(dq.conj(), sc_ast_lf_r)
        sc_ast_lf_r[0] *= sc

        # adjust rotation
        sc_ast_lf_q = dq.conj() * sc_ast_lf_q

        # adjust sun vect
        ast_sun_lf_u = tools.q_times_v(dq.conj(), ast_sun_lf_u)

        return sc_ast_lf_r, sc_ast_lf_q, ast_sun_lf_u

    def set_cropped_system_scf(self, x, y, w, h, sc_ast_lf_r, sc_ast_lf_q, rotate_sc=False):
        sc, dq = self.cropped_system_tf(x, y, w, h)

        # adjust and set position
        sc_ast_lf_r[0] /= sc
        self.spacecraft_pos = tools.q_times_v(SystemModel.sc2gl_q.conj() * dq, sc_ast_lf_r)

        # adjust and set rotation
        if rotate_sc:
            self.spacecraft_q = self.asteroid_q * dq * sc_ast_lf_q.conj()  # TODO: check that valid
        else:
            self.asteroid_q = self.spacecraft_q * dq * sc_ast_lf_q

    def cropped_system_tf(self, x, y, w, h):
        # for distance adjustment
        oh, ow = self.cam.height, self.cam.width
        sc = max(h / oh, w / ow)

        # crop to same aspect ratio, center the area
        th, tw = oh * sc, ow * sc
        ty, tx = y - (th - h) / 2, x - (tw - w) / 2

        # don't rotate centre of crop area to view if any of the sides coincide with original bounds
        if tx + tw >= ow:
            tx = ow - tw
        elif tx <= 0:
            tx = 0

        if ty + th >= oh:
            ty = oh - th
        elif ty <= 0:
            ty = 0

        # for rotation adjustment
        if False:
            # px are on a plane
            dx = math.atan(
                ((x + w / 2) - self.cam.width // 2) / (self.cam.width / 2) * math.tan(math.radians(sm.cam.x_fov / 2)))
            dy = math.atan(
                ((y + h / 2) - self.cam.height // 2) / (self.cam.height / 2) * math.tan(math.radians(sm.cam.y_fov / 2)))
        else:
            # px are on a curved surface
            dx = ((tx + tw / 2) - ow // 2) / ow * math.radians(self.cam.x_fov)
            dy = ((ty + th / 2) - oh // 2) / oh * math.radians(self.cam.y_fov)
        dq = tools.ypr_to_q(-dy, -dx, 0)

        return sc, dq

    def random_state(self, uniform_distance=True, opzone_only=False, max_sc_lateral_disp=1.0):
        # reset asteroid axis to true values
        self.asteroid.reset_to_defaults()
        self.asteroid_rotation_from_model()

        for i in range(100):
            ## sample params from suitable distributions
            ##
            # datetime dist: uniform, based on rotation period
            time = np.random.uniform(*self.time.range)

            # spacecraft position relative to asteroid in ecliptic coords:
            sc_lat = np.random.uniform(-math.pi / 2, math.pi / 2)
            sc_lon = np.random.uniform(-math.pi, math.pi)

            # s/c distance as inverse uniform distribution
            if uniform_distance:
                sc_r = np.random.uniform(self.min_distance, self.max_distance)
            else:
                sc_r = 1 / np.random.uniform(1 / self.max_distance, 1 / self.min_distance)

            # same in cartesian coord
            sc_ex_u, sc_ey_u, sc_ez_u = spherical_to_cartesian(sc_r, sc_lat, sc_lon)
            sc_ex, sc_ey, sc_ez = sc_ex_u.value, sc_ey_u.value, sc_ez_u.value

            # s/c to asteroid vector
            sc_ast_v = -np.array([sc_ex, sc_ey, sc_ez])

            # sc orientation: uniform, center of asteroid at edge of screen
            if opzone_only:
                # always get at least 50% of astroid in view, 5% of the time maximum offset angle
                max_angle = rad(min(self.cam.x_fov, self.cam.y_fov) / 2)
                da = min(max_angle, max_sc_lateral_disp * np.abs(np.random.normal(0, max_angle / 2)))
                dd = np.random.uniform(0, 2 * math.pi)
                sco_lat = tools.wrap_rads(-sc_lat + da * math.sin(dd))
                sco_lon = tools.wrap_rads(math.pi + sc_lon + da * math.cos(dd))
                sco_rot = np.random.uniform(-math.pi, math.pi)  # rotation around camera axis
            else:
                # follows the screen edges so that get more partial views, always at least 25% in view
                # TODO: add/subtract some margin
                sco_lat = tools.wrap_rads(-sc_lat)
                sco_lon = tools.wrap_rads(math.pi + sc_lon)
                sco_rot = np.random.uniform(-math.pi, math.pi)  # rotation around camera axis
                sco_q = tools.ypr_to_q(sco_lat, sco_lon, sco_rot)

                ast_ang_r = math.atan(
                    self.asteroid.mean_radius / 1000 / sc_r)  # if asteroid close, allow s/c to look at limb
                dx = max(max_sc_lateral_disp * rad(self.cam.x_fov / 2), ast_ang_r)
                dy = max(max_sc_lateral_disp * rad(self.cam.y_fov / 2), ast_ang_r)
                disturbance_q = tools.ypr_to_q(np.random.uniform(-dy, dy), np.random.uniform(-dx, dx), 0)
                sco_lat, sco_lon, sco_rot = tools.q_to_ypr(sco_q * disturbance_q)

            sco_q = tools.ypr_to_q(sco_lat, sco_lon, sco_rot)

            # sc_ast_p ecliptic => sc_ast_p open gl -z aligned view
            sc_pos = tools.q_times_v((sco_q * self.sc2gl_q).conj(), sc_ast_v)

            # get asteroid position so that know where sun is
            # *actually barycenter, not sun
            as_v = self.asteroid.position(time)
            elong, direc = tools.solar_elongation(as_v, sco_q)

            # limit elongation to always be more than set elong
            if elong > rad(self.min_elong):
                break

        if elong <= rad(self.min_elong):
            assert False, 'probable infinite loop'

        # put real values to model
        self.time.value = time
        self.spacecraft_pos = sc_pos
        self.spacecraft_rot = (deg(sco_lat), deg(sco_lon), deg(sco_rot))

        # save real values so that can compare later
        self.time.real_value = self.time.value
        self.real_spacecraft_pos = self.spacecraft_pos
        self.real_spacecraft_rot = self.spacecraft_rot
        self.real_asteroid_axis = self.asteroid_axis

        # get real relative position of asteroid model vertices
        self.asteroid.real_sc_ast_vertices = self.sc_asteroid_vertices()

    def export_state(self, filename):
        """ saves state in an easy to access format """

        qn = ('w', 'x', 'y', 'z')
        vn = ('x', 'y', 'z')
        lines = [['type'] + ['ast_q' + i for i in qn] + ['sc_q' + i for i in qn]
                 + ['ast_sc_v' + i for i in vn] + ['sun_ast_v' + i for i in vn]]

        for t in ('initial', 'real'):
            # if settings.USE_ICRS, all in solar system barycentric equatorial frame
            ast_q = self.asteroid.rotation_q(self.time.value)
            sc_q = self.spacecraft_q
            ast_sc_v = tools.q_times_v(sc_q, self.spacecraft_pos)
            sun_ast_v = self.asteroid.position(self.time.value)

            lines.append((t,) + tuple('%f' % f for f in (tuple(ast_q.components) + tuple(sc_q.components)
                                                         + tuple(ast_sc_v) + tuple(sun_ast_v))))
            self.swap_values_with_real_vals()

        with open(filename, 'w') as f:
            f.write('\n'.join(['\t'.join(l) for l in lines]))

    def propagate(self, dt):
        # save current state to past states list before propagating
        self.state_history.append({n: p.real_value for n, p in self.get_params(all=True)})

        # TODO: propagate system state based on current parameters, currently pose change done elsewhere (testloop.py)
        self.time.real_value += dt
        pass

    def save_state(self, filename, printout=False):
        config = configparser.ConfigParser()
        filename = filename + ('.lbl' if len(filename) < 5 or filename[-4:] != '.lbl' else '')
        config.read(filename)
        if not config.has_section('main'):
            config.add_section('main')
        if not config.has_section('real'):
            config.add_section('real')

        for n, p in self.get_params(all=True):
            config.set('main', n, str(p.value))
            if p.real_value is not None:
                config.set('real', n, str(p.real_value))

        if self.asteroid.real_position is not None:
            config.set('real', 'sun_asteroid_pos', str(self.asteroid.real_position))

        if not printout:
            with open(filename, 'w') as f:
                config.write(f)
        else:
            config.write(sys.stdout)

    def load_state(self, filename, sc_ast_vertices=False):
        if not os.path.isfile(filename):
            raise FileNotFoundError(filename)

        self.asteroid.reset_to_defaults()

        config = configparser.ConfigParser()
        filename = filename + ('.lbl' if len(filename) < 5 or filename[-4:] != '.lbl' else '')
        config.read(filename)

        for n, p in self.get_params(all=True):
            v = float(config.get('main', n))
            if n == 'time':
                rp = self.asteroid.rotation_period
                p.range = (v - rp / 2, v + rp / 2)
            p.value = v

            rv = config.get('real', n, fallback=None)
            if rv is not None:
                p.real_value = float(rv)

        rv = config.get('real', 'sun_asteroid_pos', fallback=None)
        if rv is not None:
            self.asteroid.real_position = np.fromstring(rv[1:-1], dtype=np.float, sep=' ')

        assert np.isclose(self.time.value, float(config.get('main', 'time'))), \
            'Failed to set time value: %s vs %s' % (self.time.value, float(config.get('main', 'time')))

        self.update_asteroid_model()

        if sc_ast_vertices:
            # get real relative position of asteroid model vertices
            self.asteroid.real_sc_ast_vertices = self.sc_asteroid_vertices(real=True)

    @staticmethod
    def frm_conv_q(fsrc, fdst, ast=None):
        fqm = {
            SystemModel.OPENGL_FRAME: np.quaternion(1, 0, 0, 0),
            SystemModel.OPENCV_FRAME: SystemModel.cv2gl_q,
            SystemModel.SPACECRAFT_FRAME: SystemModel.sc2gl_q,
            SystemModel.ASTEROID_FRAME: None if ast is None else ast.ast2sc_q * SystemModel.sc2gl_q,
        }
        return fqm[fsrc] * fqm[fdst].conj()

    def __repr__(self):
        return (
                       'system state:\n\t%s\n'
                       + '\nsolar elongation: %s\n'
                       + '\nasteroid rotation: %.2f\n'
               ) % (
                   '\n\t'.join('%s = %s' % (n, p) for n, p in self.get_params(all=True)),
                   tuple(map(math.degrees, self.solar_elongation())),
                   math.degrees(self.asteroid.rotation_theta(self.time.value)),
               ) class ImageProc:
    latest_opt = None
    show_fit = None

    @staticmethod
    def add_noise_to_image(image, noise_img_file):
        tmp = cv2.imread(noise_img_file, cv2.IMREAD_UNCHANGED)
        noise_img = cv2.resize(tmp, None,
                               fx=image.shape[1] / tmp.shape[1],
                               fy=image.shape[0] / tmp.shape[0],
                               interpolation=cv2.INTER_CUBIC)

        return cv2.add(image, noise_img[:, :, 3])

    @staticmethod
    def crop_and_zoom_image(image, x_off, y_off, width, height, scale, trg_w_h=None, others=tuple()):
        tw, th = trg_w_h
        if scale is None:
            scale = min(th / height, tw / width)

        res = []
        for img in [image] + list(others):
            imgc = cv2.resize(img[y_off:y_off + height, x_off:x_off + width], None, fx=scale, fy=scale,
                              interpolation=cv2.INTER_AREA)
            oh, ow = img.shape
            ch, cw = imgc.shape

            if trg_w_h is not None:
                if x_off + width >= ow:
                    x0 = tw - cw
                elif x_off <= 0:
                    x0 = 0
                else:
                    x0 = (tw - cw) // 2

                if y_off + height >= oh:
                    y0 = th - ch
                elif y_off <= 0:
                    y0 = 0
                else:
                    y0 = (th - ch) // 2

                imgd = np.zeros((th, tw), dtype=img.dtype)
                imgd[y0:y0 + ch, x0:x0 + cw] = imgc
            else:
                imgd = imgc
            res.append(imgd)

        if len(others) > 0:
            return res
        return res[0]

    @staticmethod
    def single_object_bounds(img, threshold, crop_marg, min_px, debug=False):
        # binary image
        _, mask = cv2.threshold(img, threshold, 255, cv2.THRESH_BINARY)

        # remove stars
        mask = cv2.erode(mask, ImageProc.bsphkern(9), iterations=1)

        if np.sum(mask) < min_px:
            return (None,) * 4

        # detect target
        x_, y_, w_, h_ = cv2.boundingRect(mask)

        # add margin
        x, y = max(0, x_ - crop_marg), max(0, y_ - crop_marg)
        w = min(mask.shape[1] - x, w_ + 2 * crop_marg - (x - x_ + crop_marg))
        h = min(mask.shape[0] - y, h_ + 2 * crop_marg - (y - y_ + crop_marg))

        if debug:
            img_color = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
            img_color = cv2.rectangle(img_color, (x, y), (x + w, y + h), (0, 0, 255), thickness=1)
            img_color[y + h // 2, x + w // 2] = (0, 0, 255)
            cv2.imshow('box', cv2.resize(img_color, (512, 512)))

        return x, y, w, h

    @staticmethod
    def equalize_brightness(image, ref_image, percentile=98, image_gamma=1):
        image = ImageProc.adjust_gamma(image, 1 / image_gamma)
        ip = np.percentile(image, percentile)
        rp = np.percentile(ImageProc.adjust_gamma(ref_image, 1 / image_gamma), percentile)
        image = cv2.convertScaleAbs(image, None, rp / ip, 0)
        return ImageProc.adjust_gamma(image, image_gamma)

    @staticmethod
    def normalize_brightness(image, quantiles=(0.0005, 0.9999), top_margin=1.2, src_gamma=1.0, gamma=1.0):
        image = ImageProc.adjust_gamma(image, src_gamma, inverse=True)

        bot_v, top_v = np.quantile(image, quantiles)
        top_v = top_v * top_margin

        if image.dtype == np.uint8:
            sc = 255 / (top_v - bot_v)
            image = cv2.convertScaleAbs(image, None, sc, -bot_v * sc)
        elif image.dtype in (float, np.float32):
            sc = 1 / (top_v - bot_v)
            image = np.clip((image - bot_v) * sc, 0, 1)
        else:
            assert False, 'unsupported image dtype: %s' % image.dtype

        return ImageProc.adjust_gamma(image, gamma)

    @staticmethod
    def default_preprocess(image, max=255):
        bg = np.percentile(image, 250 / 1024 * 100)
        return ImageProc.adjust_gamma(np.clip((image - bg) * max / (max - bg), 0, max), 1.8)

    @staticmethod
    def change_color_depth(img, src_bits, dst_bits):
        if src_bits == dst_bits:
            return img

        if str(img.dtype)[:4] == 'uint':
            new_type = 'uint' + str(math.ceil(dst_bits / 8) * 8)
        else:
            new_type = img.dtype

        if src_bits < dst_bits:
            img = img.astype(new_type)

        img = img * (2 ** (dst_bits - src_bits))

        if src_bits > dst_bits:
            img = img.astype(new_type)

        return img

    @staticmethod
    def remove_bg(img, bg_img, gain=None, max_val=None, offset=0):
        if gain is None:
            # estimate correct gain
            cost_fun = lambda g: np.var((img - g[0] * bg_img).reshape((-1, 3)), axis=0)
            x, _ = leastsq(cost_fun, np.array([1]))
            gain = x[0]
            print('estimated bg gain: %f' % gain)

        imgr = img.astype('float') - gain * bg_img
        if offset not in (None, False):
            imgr += offset - np.min(imgr)
        if max_val and offset is not False:
            return np.clip(imgr, 0, max_val).astype(img.dtype)
        return imgr

    @staticmethod
    def color_correct(img, bgr_mx, inverse=False, max_val=None):
        assert img.shape[2] == 3, 'can only do to BGR images'
        if inverse:
            bgr_mx = np.linalg.inv(bgr_mx)

        imgc = bgr_mx.dot(img.reshape((-1, 3)).T).T.reshape(img.shape)
        if max_val:
            return np.clip(imgc, 0, max_val).astype(img.dtype)
        return imgc

    @staticmethod
    def adjust_gamma(image, gamma, gamma_break=None, linear_part=True, inverse=False, max_val=255):
        if gamma == 1:
            return image

        # build a lookup table mapping the pixel values [0, 255] to
        # their adjusted gamma values
        invGamma = gamma if inverse else 1.0 / gamma
        gamma_break = gamma_break or 0

        if image.dtype == 'uint8' and gamma_break == 0:
            # apply gamma correction using the lookup table
            max_val = min(max_val, 255)
            table = np.array([((i / max_val) ** invGamma) * max_val for i in np.arange(0, max_val + 1)]).astype(
                image.dtype)
            adj_img = cv2.LUT(image, table)
        elif gamma_break == 0:
            adj_img = np.round(((image / max_val) ** invGamma) * max_val).astype(image.dtype)
        elif True:
            # from https://se.mathworks.com/help/vision/ref/gammacorrection.html
            b_p = gamma_break
            s_ls = 1 / (gamma / b_p ** (1 / gamma - 1) - gamma * gamma_break + gamma_break)
            f_s = gamma * s_ls / b_p ** (1 / gamma - 1)
            c_o = f_s * b_p ** (1 / gamma) - s_ls * b_p

            img = image.flatten() / max_val
            I = img <= (s_ls if inverse else 1) * b_p
            nI = np.logical_not(I)

            adj_img = np.zeros(image.shape).flatten()
            adj_img[I] = (img[I] / s_ls) if inverse else (img[I] * s_ls)
            adj_img[nI] = (((img[nI] + c_o) / f_s) ** gamma) if inverse else (f_s * img[nI] ** (1 / gamma) - c_o)
            adj_img = (adj_img * max_val).reshape(image.shape).astype(image.dtype)

        else:
            # from https://en.wikipedia.org/wiki/SRGB
            if 1:
                a = gamma_break
                K0 = a / (gamma - 1)
            else:
                K0 = gamma_break
                a = K0 * (gamma - 1)

            alpha = 1 + a
            th = alpha ** gamma * (gamma - 1) ** (gamma - 1) / a ** (gamma - 1) / gamma ** gamma

            lim = K0 if inverse else K0 / th
            img = image.flatten() / max_val
            I = img <= lim
            nI = np.logical_not(I)

            adj_img = np.zeros(image.shape).flatten()
            adj_img[I] = (img[I] / th) if inverse else (th * img[I])
            adj_img[nI] = (((img[nI] + a) / alpha) ** gamma) if inverse else (alpha * img[nI] ** (1 / gamma) - a)
            adj_img = (adj_img * max_val).reshape(image.shape).astype(image.dtype)
            # adj_img = np.round(adj_img * max_val).reshape(image.shape).astype(image.dtype)

        return adj_img

    @staticmethod
    def apply_point_spread_fn(img, ratio):
        # ratio is how many % of power on central pixel
        sd = 1 / math.sqrt(2 * math.pi * ratio)
        size = 1 + 2 * math.ceil(sd * 2)
        kernel = ImageProc.gkern2d(size, sd)
        cv2.filter2D(img, -1, kernel, img)
        return img

    @staticmethod
    @lru_cache(maxsize=5)
    def gkern2d(l=5, sig=1.):
        """
        creates gaussian kernel with side length l and a sigma of sig
        """
        w, h = (l[0], l[1]) if '__iter__' in dir(l) else (l, l)
        sx, sy = (sig[0], sig[1]) if '__iter__' in dir(sig) else (sig, sig)
        ax = np.arange(-w // 2 + 1., w // 2 + 1.)
        ay = np.arange(-h // 2 + 1., h // 2 + 1.)
        xx, yy = np.meshgrid(ax, ay)
        kernel = np.exp(-((xx / sx) ** 2 + (yy / sy) ** 2) / 2)
        return kernel / np.sum(kernel)

    @staticmethod
    def bsphkern(l=5):
        """
        creates a binary spherical kernel
        """
        gkern = ImageProc.gkern2d(l=l, sig=l)
        limit = gkern[l // 2 if isinstance(l, int) else l[1] // 2, -1] * 0.995
        return np.array(gkern >= limit, dtype=np.uint8)

    @staticmethod
    def fuzzy_kernel(kernel, sig):
        w = int(sig // 2)
        skernel = np.zeros(tuple(np.array(kernel.shape[:2]) + int(sig)) + kernel.shape[2:3], dtype=kernel.dtype)
        skernel[w:w + kernel.shape[0], w:w + kernel.shape[1]] = kernel
        gkrn = ImageProc.gkern2d(sig, sig / 2)
        skernel = cv2.filter2D(skernel, kernel.shape[2], gkrn)
        return skernel

    @staticmethod
    def _img_max_valid(img):
        max = 1.0 if 'float' in str(img.dtype) else 255
        assert max != 255 or img.dtype == np.uint8, 'wrong datatype for image: %s' % img.dtype
        return max

    @staticmethod
    def add_stars(img, mask, coef=2, cache=False):
        # add power law distributed stars to image
        assert img.shape == img.shape[:2], 'works only with grayscale images'
        if not cache:
            ImageProc._cached_random_stars.cache_clear()
        stars = ImageProc._cached_random_stars(coef, img.shape)
        # can be over 255, will clip later
        img[mask] = np.clip(stars[mask], 0, 600)
        return img

    @staticmethod
    @lru_cache(maxsize=1)
    def _cached_random_stars(coef, shape):
        return np.random.pareto(coef, shape)

    @staticmethod
    def add_sensor_noise(img, mean=7, sd=2, cache=False):
        if not cache:
            ImageProc._cached_sensor_noise.cache_clear()
        img += ImageProc._cached_sensor_noise(mean, sd, img.shape)
        return img

    @staticmethod
    @lru_cache(maxsize=1)
    def _cached_sensor_noise(mean, sd, shape):
        return np.random.normal(mean, sd, shape)

    @staticmethod
    def process_target_image(image_src):
        hist = cv2.calcHist([image_src], [0], None, [256], [0, 256])
        if False:
            threshold_value = ImageProc.optimal_threshold(hist)
        else:
            threshold_value = 50
        th, image_dst = cv2.threshold(image_src, threshold_value, 255, cv2.THRESH_TOZERO)
        return image_dst, hist, threshold_value

    @staticmethod
    def optimal_threshold(hist, image=None):
        if hist is None:
            hist = cv2.calcHist([image], [0], None, [256], [0, 256])

        tot_px = 256  # sum(hist) -- for some reason get error if divide with pixel count
        x = list(range(1, len(hist) + 1))
        loghist = np.array(list(map(lambda x: math.log(x + 1) / tot_px, hist)))

        def fitfun1(p, x):
            return stats.gamma.pdf(x, p[0], loc=0, scale=p[1]) * p[2]

        def fitfun2(p, x):
            return stats.norm.pdf(x, p[0], p[1]) * p[2]

        def fitfun(p, x):
            return fitfun1(p[:3], x) + fitfun2(p[3:], x)

        def errfun(p, x, y):
            tmp = y - fitfun(p, x)
#            assert False, 'p:%s, x:%s, y:%s, ffval:%s'%(p, x[0:50], y[0:50], fitfun(p, x[0:50]))
            return tmp

        shape = 1.5
        init = [
            shape, np.argmax(loghist) / (shape - 1), 1,  # for fitfun1
            127, 50, 1,  # for fitfun2
        ]
        if not BATCH_MODE or DEBUG:
            print('init: %s' % init)

        out = optimize.leastsq(errfun, init, args=(x, loghist))
        ImageProc.latest_opt = out
        if not BATCH_MODE or DEBUG:
            print('result: %s' % list(out))

        # threshold value where background makes up roughly a fourth of all pixels
        bg = reversed(fitfun1(out[0][:3], x))
        ast = list(reversed(fitfun2(out[0][3:], x)))
        threshold_value = 255 - next((i for i, v in enumerate(bg) if v / ast[i] > 0.33), 255 - 100)

        if not BATCH_MODE or DEBUG:
            bg_ratio = out[0][:3][2] / out[0][3:][2]
            print('threshold_value: %s; bg_ratio: %s' % (threshold_value, bg_ratio))

        # plot figure with histogram and estimated distributions
        if DEBUG:
            from matplotlib import pyplot as plt
            plt.clf()
            plt.plot(x, fitfun1(out[0][:3], x), label='background')
            plt.plot(x, fitfun2(out[0][3:], x), label='foreground')
            plt.plot(x, fitfun(out[0], x), label='total fit')
            plt.plot(x, loghist, label='log(hist)')
            plt.legend()
            fig = plt.gcf()
            fig.canvas.draw()
            data = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8)
            w, h = fig.canvas.get_width_height()
            data = data.reshape((h * 3, w * 3, 3))  # for some reason get_width_height returns 1/3 of the actual dims
            cv2.imshow('histogram fitting', data)

        return threshold_value

    @staticmethod
    def overlay_mask(image, mask):
        sc_img = min(image.shape[0], mask.shape[0])/image.shape[0]
        sc_mask = min(image.shape[0], mask.shape[0])/mask.shape[0]
        img_color = cv2.cvtColor(cv2.resize(image, None, fx=sc_img, fy=sc_img, interpolation=cv2.INTER_CUBIC), cv2.COLOR_GRAY2RGB)
        mask_color = cv2.cvtColor(cv2.resize((mask > 0).astype(np.uint8)*255, None, fx=sc_mask, fy=sc_mask, interpolation=cv2.INTER_CUBIC), cv2.COLOR_GRAY2RGB)
        mask_color[:, :, 0:2] = 0
        return cv2.addWeighted(img_color, 0.5, mask_color, 0.5, 0.0)

    @staticmethod
    def merge(images):
        summed_weights = 1
        summed_images = images[0]
        for i in range(1, len(images)):
            summed_images = cv2.addWeighted(summed_images, summed_weights / (summed_weights + 1),
                                            images[i], 1 / (summed_weights + 1), 0.0)
            summed_weights += 1
        return summed_images

    @staticmethod
    def norm_xcorr(sce_img, res_img):
        """ calculate normalized cross corralation of images """

        if sce_img.shape[:2] != res_img.shape[:2]:
            sce_img = cv2.resize(sce_img, None,
                                 fx=res_img.shape[1] / sce_img.shape[1],
                                 fy=res_img.shape[0] / sce_img.shape[0],
                                 interpolation=cv2.INTER_CUBIC)

        sce_img = np.atleast_3d(sce_img)
        res_img = np.atleast_3d(res_img)

        sce_mean, sce_std = cv2.meanStdDev(sce_img)
        res_mean, res_std = cv2.meanStdDev(res_img)
        stds = sce_std * res_std
        if stds == 0:
            return 0

        corr = (sce_img - sce_mean) * (res_img - res_mean)
        nxcorr = np.mean(corr) / stds

        if False:
            # for debugging
            tmp = np.log(corr - np.min(corr) + 0.001)
            mint = np.min(tmp)
            maxt = np.max(tmp)
            tmp = (tmp - mint) * (1 / (maxt - mint))

            print('sm %.3f, ss %.3f, rm %.3f, rs %.3f, min %.3f, max %.3f, res %.3f' % (
            sce_mean, sce_std, res_mean, res_std, mint, maxt, nxcorr))
            cv2.imshow('corr', tmp)
            cv2.waitKey()

        return nxcorr class PhaseCorrelationAlgo(AlgorithmBase):
    def __init__(self, system_model, render_engine, obj_idx):
        super(PhaseCorrelationAlgo, self).__init__(system_model, render_engine, obj_idx)

        self.min_options = None
        self.errval0 = None
        self.errval1 = None
        self.errval  = None
        self.iter_count = 0
        self.start_time = None
        self.extra_values = []

        self._image = None
        self._image_file = None
        self._target_image = None
        self._hannw = None
        self._render_result = None
        self._count = 0
    
    def optfun(self, *args):
        if any(map(math.isnan, args)):
            raise PositioningException('Position resulted in a NAN: %s'(sc_v,))
        
        # disable update events fired when parameters are changed
        #self.system_model.param_change_events(False)
        
        for (n, p), v in zip(self.system_model.get_params(), args):
            p.nvalue = v
            
        self.iter_count += 1
        err = self.errfun()

        # log result
        self.extra_values.append(err)
        
        # enable events again
        #self.system_model.param_change_events(True)
        return err

    def errfun(self, render_result=None):
        # value that bounds the input parameters, kind of weak priors
        # for a bayasian thing
        # TODO: actual priors based on centroid algo
        m = self.system_model
        self.errval0 = (0
#            + (m.x_off.nvalue)**2
#            + (m.y_off.nvalue)**2
#            + (m.z_off.nvalue)**2
#            + ((m.x_off.nvalue - m.x_off.def_val)**2 if not m.x_off.valid else 0)
#            + ((m.y_off.nvalue - m.y_off.def_val)**2 if not m.y_off.valid else 0)
#            + ((m.z_off.nvalue - m.z_off.def_val)**2 if not m.z_off.valid else 0)
#            + 10 * (m.time.nvalue)**2
#            ((m.x_rot.nvalue)**2 if m.x_rot.valid else 0) +
#            ((m.y_rot.nvalue)**2 if m.y_rot.valid else 0) +
#            ((m.z_rot.nvalue)**2 if m.z_rot.valid else 0)
        )
        
        if render_result is None:
            render_result = self.render()
            #imgfile = self.debug_filebase+('i%.0f'%(self.iter_count))+'.png'
            #cv2.imwrite(imgfile, render_result)
            self._count+=1

        self._render_result = render_result
#        cv2.imshow('render_result', render_result)
#        cv2.imshow('target_image', self._img)
#        cv2.waitKey()
        
        assert render_result.shape==self._target_image.shape, (
            'phase corr params: %s, %s\n'
            + 'im_xoff:%s, im_yoff:%s, '
            + 'im_width:%s, im_height:%s, im_scale:%.3f, '
            + 'vw_width:%s, vw_height:%s\n') % (
                render_result.shape, self._target_image.shape,
                self.im_xoff, self.im_yoff,
                self.im_width, self.im_height, self.im_scale,
                m.view_width, m.view_height)
        (tcx, tcy), pwr = cv2.phaseCorrelate(
                np.float32(render_result), self._target_image, self._hannw)
        
        # if crop params set, compensate cx, cy
        xi, yi = self._original_image_coords(
                tcx + m.view_width/2,
                tcy + m.view_height/2)
        
        scx, scy = self._cam.calc_xy(xi, yi, m.z_off.value)
        m.spacecraft_pos = (scx, scy, m.z_off.value)
        
        #print('z=%.3f: (%.3f, %.3f) => (%.3f, %.3f) => (%.3f, %.3f) => (%.3f, %.3f)'%(
        #    -m.z_off.value, tcx, tcy, cxc, cyc, cx, cy, m.x_off.value, m.y_off.value))
        
        self.errval1 = -pwr
        self.errval = self.errval0 + self.errval1

        if not BATCH_MODE or DEBUG:
            print((
                'E[%.3f] | (%.2f, %.2f) | %.2f | %.2f | %.2f > '+
                    '%.2f | %.2f | %.2f | %.2f < %s')%(
                    self.errval1, -xi, -yi,
                    m.x_rot.nvalue, m.y_rot.nvalue, m.z_rot.nvalue,
                    m.x_off.nvalue, m.y_off.nvalue, m.z_off.nvalue, m.time.nvalue,
                    '%s'%self.iter_count if self.iter_count>0 else '-'
                )
            )

        return self.errval
    
    def _get_bounds(self, img, margin, max_width):

        # get x, y, w, h from img
        xs, ys, ws, hs = cv2.boundingRect(img)

        cw, ch = self._cam.width, self._cam.height

        # translate to 1:1 scale, add margin
        tmpw = round(ws/self.im_scale) +2*margin
        tmph = round(hs/self.im_scale) +2*margin
        im_width = min(cw, max(tmpw, round(tmph * cw/ch)))
        im_height = min(ch, max(tmph, round(tmpw * ch/cw)))
        
        (x, y) = self._original_image_coords(xs, ys)
        im_xoff = round(max(0, min(x - margin, cw - im_width)))
        im_yoff = round(max(0, min(y - margin, ch - im_height)))
        
        if DEBUG:
            print('(%.0f, %.0f, %.0f, %.0f) => (%.1f, %.1f, %.1f, %.1f)'%(
                xs, ys, ws, hs, im_xoff, im_yoff, im_width, im_height
            ))
        
        # see if still need to down scale
        im_scale = min(1, max_width/im_width)
        
        # adjust bounds to cover whole view
        if im_scale == 1:
            view_w = self.system_model.view_width
            view_h = self.system_model.view_height
            im_xoff = max(0, min(im_xoff-round((view_w-im_width)/2), cw-view_w))
            im_yoff = max(0, min(im_yoff-round((view_h-im_height)/2), ch-view_h))
            im_width = view_w
            im_height = view_h
        
        return im_xoff, im_yoff, im_width, im_height, im_scale
    
    def _original_image_coords(self, cx, cy):
        return tuple(c/self.im_scale + o for c, o in
                    ((cx, self.im_xoff), (cy, self.im_yoff)))

    def findstate(self, imgfile, outfile, min_options={}, **kwargs):
        self.debug_filebase = outfile        
        min_options = dict(min_options)
        
        if self._image_file != imgfile or self._image is None:
            self._image = self.load_target_image(imgfile)
            self._image_file = imgfile

        self._target_image = np.float32(self._image)
        
        # consider if useful at all
        if kwargs.pop('centroid_init', False):
            try:
                CentroidAlgo.update_sc_pos(
                        self.system_model, self._image)
                ok = True
            except PositioningException as e:
                if not e.args or e.args[0] != 'No asteroid found':
                    print(str(e))
                print('|', end='', flush=True)
                ok = False
            if not ok:
                return False
        
        self.extra_values = []
        
        #hwsize = kwargs.get('hwin_size', 4)
        #tmp = cv2.createHanningWindow((hwsize, hwsize), cv2.CV_32F)
        #sd = int((self._cam.height - hwsize)/2)
        #self._hannw = cv2.copyMakeBorder(tmp,
        #        sd, sd, sd, sd, cv2.BORDER_CONSTANT, 0)
        
        self._hannw = None
        self.start_time = datetime.now()
        method = min_options.pop('method', False)
        init_vals = np.array(list(p.nvalue for n, p in
                                  self.system_model.get_params()))
        
        if method=='simplex':
            options={'maxiter':100, 'xtol':2e-2, 'ftol':5e-2}
            options.update(min_options)
            res = optimize.minimize(lambda x: self.optfun(*x), init_vals,
                                    method='Nelder-Mead', options=options)
            x = res.x
        elif method=='powell':
            options = {'maxiter':100, 'xtol':2e-2, 'ftol':5e-2,
                       'direc':np.identity(len(init_vals))*0.01}
            options.update(min_options)
            res = optimize.minimize(lambda x: self.optfun(*x), init_vals,
                                    method='Powell', options=options)
            x = res.x
        elif method=='cobyla':
            options = {'maxiter':100, 'rhobeg': 0.1}
            options.update(min_options)
            res = optimize.minimize(lambda x: self.optfun(*x), init_vals,
                                    method='COBYLA', options=options)
            x = res.x
        elif method=='cg':
            options = {'maxiter':1000, 'eps':0.01, 'gtol':1e-4}
            options.update(min_options)
            res = optimize.minimize(lambda x: self.optfun(*x), init_vals,
                                    method='CG', options=options)
            x = res.x
        elif method=='bfgs':
            options = {'maxiter':1000, 'eps':0.01, 'gtol':1e-4}
            options.update(min_options)
            res = optimize.minimize(lambda x: self.optfun(*x), init_vals,
                                    method='BFGS', options=options)
            x = res.x
        elif method=='anneal':
            options = {
                'niter':350, 'T':1.25, 'stepsize':0.3,
                'minimizer_kwargs':{
#                    'method':'Nelder-Mead',
#                    'options':{'maxiter':25, 'xtol':2e-2, 'ftol':1e-1}
                    'method':'COBYLA',
                    'options':{'maxiter':25, 'rhobeg': 0.1},
            }}
            options.update(min_options)
            res = optimize.basinhopping(lambda x: self.optfun(*x),
                                        init_vals, **options)
            x = res.x
        elif method=='brute':
            min_opts = dict(min_options)
            max_iter = min_opts.pop('max_iter', 1000)
            init = list((-0.5, 0.5) for n, p in self.system_model.get_params())
            options = {'Ns':math.floor(math.pow(max_iter, 1/len(init))),
                       'finish':None}
            options.update(min_opts)
            optfun = self.optfun if len(init)==1 else lambda x: self.optfun(*x)
            x = res = optimize.brute(optfun, init, **options)
            x = (x,) if len(init)==1 else x
                
        elif method=='two-step-brute':
            min_opts = dict(min_options)
            
            ## PHASE I
            ## >> 
            first_opts = dict(min_opts.pop('first'))

            im_scale = first_opts.pop('scale', self.im_def_scale)
            self.set_image_zoom_and_resolution(im_scale=im_scale)

            max_iter = first_opts.pop('max_iter', 50)
            init = list((-0.5, 0.5) for n, p in self.system_model.get_params())
            optfun = self.optfun if len(init)==1 else lambda x: self.optfun(*x)            
            options = {'Ns':math.floor(math.pow(max_iter, 1/len(init))),
                       'finish':None}
            options.update(first_opts)
            x = res = optimize.brute(optfun, init, **options)
            x = (x,) if len(init)==1 else x
            self.iter_count = -1
            self.optfun(*x) # set sc params to result values
            ## <<
            ## PHASE I

            img = self.render()
            cv2.imwrite(self.debug_filebase+'.png', img)

            if DEBUG:
                print('Phase I: (%.3f, %.3f, %.3f)\n'%(
                    self.system_model.x_off.value,
                    self.system_model.y_off.value,
                    -self.system_model.z_off.value,
                ))
            
            ## PHASE II
            ## >>
            second_opts = dict(min_opts.pop('second'))
            margin = second_opts.pop('margin', 50)
            distance_margin = second_opts.pop('distance_margin', 0.2)
            max_width = second_opts.pop('max_width', -self.system_model.view_width)
            
            # calculate min_dist and max_dist
            min_dist = (-self.system_model.z_off.value) * (1-distance_margin)
            max_dist = (-self.system_model.z_off.value) * (1+distance_margin)

            # calculate im_xoff, im_xoff, im_width, im_height
            render_result = self.render()
            xo, yo, w, h, sc = self._get_bounds(render_result, margin, max_width)
            
            if DEBUG:
                print('min_d, max_d, xo, yo, w, h, sc: %.3f, %.3f, %.0f, %.0f, %.0f, %.0f, %.3f\n'%(
                    min_dist, max_dist, xo, yo, w, h, sc
                ))
            
            if True:
                self.debug_filebase = outfile+'r2'
                
                # limit distance range
                self.system_model.z_off.range = (-max_dist, -min_dist)
                
                # set cropping & zooming
                self.set_image_zoom_and_resolution(
                                            im_xoff=xo, im_yoff=yo,
                                            im_width=w, im_height=h, im_scale=sc)
                self._target_image = np.float32(self._image)
                img = self.render()
                cv2.imwrite(self.debug_filebase + '.png', img)

                # set optimization params
                max_iter = second_opts.pop('max_iter', 18)
                init = list((-0.5, 0.5) for n, p in self.system_model.get_params())
                optfun = self.optfun if len(init)==1 else lambda x: self.optfun(*x)
                options = {'Ns':math.floor(math.pow(max_iter, 1/len(init))),
                           'finish':None}
                options.update(second_opts)
            
                # optimize
                x = res = optimize.brute(optfun, init, **options)
                x = (x,) if len(init)==1 else x
            
        self.iter_count = -1;
        self.optfun(*x)
        outfile = imgfile[0:-4]+'r3'+'.png'
        img = self.render()
        cv2.imwrite(outfile, img)

        if method=='two-step-brute':
            self.system_model.z_off.range = (-self.system_model.max_distance, -self.system_model.min_med_distance)
            if DEBUG:
                print('Phase II: (%s, %s, %s)\n'%(
                    self.system_model.x_off.value,
                    self.system_model.y_off.value,
                    -self.system_model.z_off.value,
                ))
        
        if not BATCH_MODE or DEBUG:
            print('%s'%res)
            print('seconds: %s'%(datetime.now()-self.start_time))
            
        if self.im_def_scale != self.im_scale:
            self.set_image_zoom_and_resolution(im_scale=self.im_def_scale)
        
        return True class CentroidAlgo(AlgorithmBase):
    def __init__(self, system_model, render_engine, obj_idx, **kwargs):
        super(CentroidAlgo, self).__init__(system_model, render_engine, obj_idx)

        self._bg_threshold = 20                 # default thresholding value
        self._ref_img = None
        
        self.DEBUG_IMG_POSTFIX = 'c'            # fi batch mode, save result image in a file ending like this

        self.RENDER_SHADOWS = True
        self.RENDER_TEXTURES = True
        self.MIN_PIXELS_FOR_DETECTION = 30      # fail if less pixels lit
        self.ASTEROID_MIN_BORDER_MARGIN = 0.04  # if less than margin at both extrames, astroid too close
        self.ASTEROID_MAX_SPAN = 0.85           # if asteroid spans more than this, it's too close
        self.MAX_ITERATIONS = 30                # max number of iterations
        self.ITERATION_TOL = 0.002              # min change % in position vector
        self.CHECK_RESULT_VALIDITY = False
        self.MIN_RESULT_XCORR = 0.3             # if result xcorr with scene is less than this, fail
    
    
    def adjust_iteratively(self, sce_img, outfile=None, **kwargs):
        self.debug_filebase = outfile
        self._bg_threshold = kwargs.get('bg_threshold', self._bg_threshold)
        sce_img = self.maybe_load_scene_image(sce_img, preproc='bg_threshold' not in kwargs)

        if DEBUG:
            sc = self.system_model.view_width/sce_img.shape[1]
            cv2.imshow('target img', cv2.resize(sce_img, None, fx=sc, fy=sc))

        self.system_model.spacecraft_pos = (0, 0, -self.system_model.min_med_distance)
        for i in range(self.MAX_ITERATIONS):
            ox, oy, oz = self.system_model.spacecraft_pos
            od = math.sqrt(ox**2 + oy**2 + oz**2)
            
            if not DEBUG:
                self.adjust(sce_img, preproc=False)
            else:
                try:
                    self.adjust(sce_img, preproc=False)
                except PositioningException as e:
                    print(str(e))
                    break
                finally:
                    cv2.imshow('rendered img', self._ref_img)
                    cv2.waitKey()
            
            nx, ny, nz = self.system_model.spacecraft_pos
            ch = math.sqrt((nx-ox)**2 + (ny-oy)**2 + (nz-oz)**2)
            if DEBUG:
                print('i%d: d0=%.2f, ch=%.2f, rel_ch=%.2f%%'%(i, od, ch, ch/od*100))
            if ch/od < self.ITERATION_TOL:
                break
        
        if self.CHECK_RESULT_VALIDITY:
            result_quality = ImageProc.norm_xcorr(sce_img, self._ref_img)
            if result_quality < self.MIN_RESULT_XCORR:
                raise PositioningException('Result failed quality test with score: %.3f'%(result_quality,))
        
        if BATCH_MODE and self.debug_filebase:
            img = self.render(shadows=self.RENDER_SHADOWS, textures=self.RENDER_TEXTURES)
            cv2.imwrite(self.debug_filebase+'r.png', img)

        if DEBUG:
            cv2.waitKey()
            cv2.destroyAllWindows()
    
    
    def adjust(self, sce_img, ref_img=None, simple=False, preproc=True):
        sce_img = self.maybe_load_scene_image(sce_img, preproc=preproc)
        
        if not simple:
            if ref_img is None:
                ref_img = self.render(shadows=self.RENDER_SHADOWS, textures=self.RENDER_TEXTURES)
                th, ref_img = cv2.threshold(ref_img, self._bg_threshold, 255, cv2.THRESH_TOZERO)
                ref_img = np.atleast_3d(ref_img)
                self._ref_img = ref_img
                
            sc_v = self.match_brightness_centroids(sce_img, ref_img)
        else:
            sc_v = self.simple_adjusted_centroid(sce_img)
        
        if not BATCH_MODE and DEBUG:
            print('\nreal pos:\n%s\nest pos:\n%s\n' % (
                self.system_model.real_spacecraft_pos, sc_v))
        
        if any(map(math.isnan, sc_v)):
            raise PositioningException('Position resulted in a NAN: %s'(sc_v,))
        
        self.system_model.spacecraft_pos = sc_v
        
        
    def match_brightness_centroids(self, sce_img, ref_img):
        
        # for asteroid cross section area visible, get lit pixels (so that no need to fine tune rendering)
        t1, t2, sce_px, t3, t4 = self.get_image_centroid(sce_img, is_scene=True, binary=True)
        t1, t2, ref_px, t3, t4 = self.get_image_centroid(ref_img, binary=True)

        # for centroid, use centre of brightness
        sce_icx, sce_icy, t1, t2, t3 = self.get_image_centroid(sce_img, is_scene=True, binary=False)
        ref_icx, ref_icy, t1, t2, t3 = self.get_image_centroid(ref_img, binary=False)
        
        old_x = self.system_model.x_off.value
        old_y = self.system_model.y_off.value
        old_z = self.system_model.z_off.value

        # when adjusting for distance, remember to adjust all coordinates, try to first adjust laterally
        new_z = old_z * (math.sqrt(ref_px/sce_px) ** .5)
        err_d_x = old_x * (1 - new_z/old_z)
        err_d_y = old_y * (1 - new_z/old_z)

        # calculate brightness centroid coordinates
        ref_cx, ref_cy = self._cam.calc_xy(ref_icx, ref_icy, max(old_z, new_z))  # use max so that wont move out of view
        sce_cx, sce_cy = self._cam.calc_xy(sce_icx, sce_icy, max(old_z, new_z))  # use max so that wont move out of view

        # lateral errors
        err_x = ref_cx - sce_cx
        err_y = ref_cy - sce_cy

        # try to first adjust laterally
        if abs(err_x) + abs(err_y) > .2:
            new_z = old_z
            err_d_x = 0
            err_d_y = 0

        # move x_off & y_off by centroid error
        new_x = old_x - err_x - err_d_x
        new_y = old_y - err_y - err_d_y
        
        # return new location
        return (new_x, new_y, new_z)
     
     
    def detect_asteroid(self, sce_img):
        ih, iw = sce_img.shape[0:2]
        
        # Threshold it so it becomes binary
        ret, bin_img = cv2.threshold(sce_img, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
        
        # connect close pixels by dilating and eroding
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
        bin_img = cv2.morphologyEx(bin_img, cv2.MORPH_CLOSE, kernel)
        
        # get connected regions and their stats
        n, regs, stats, c = cv2.connectedComponentsWithStats(bin_img, 8, cv2.CV_32S)
        
        # detect asteroid
        parts = list(s[cv2.CC_STAT_AREA] for i,s in enumerate(stats) if i>0)
        if not parts or stats[np.argmax(parts)+1][cv2.CC_STAT_AREA] < self.MIN_PIXELS_FOR_DETECTION:
            raise PositioningException('No asteroid found')
        
        # asteroid parts (i=0 is background)
        ast_parts = (
                i for i,s in enumerate(stats)
                if i>0 and s[cv2.CC_STAT_AREA]>=self.MIN_PIXELS_FOR_DETECTION
        )
        
        # check asteroid extent
        ast_parts = [
            (
                i, s[cv2.CC_STAT_AREA],
                s[cv2.CC_STAT_LEFT]/iw, (iw - (s[cv2.CC_STAT_LEFT]+s[cv2.CC_STAT_WIDTH]))/iw,
                s[cv2.CC_STAT_TOP]/ih, (ih - (s[cv2.CC_STAT_TOP]+s[cv2.CC_STAT_HEIGHT]))/ih
            )
            for i,s in enumerate(stats) if i>0 and s[cv2.CC_STAT_AREA]>=self.MIN_PIXELS_FOR_DETECTION]

        tot_area = sum((p[1] for p in ast_parts))/iw/ih
        t1, t2, lm, rm, tm, bm = np.min(ast_parts, axis=0)
        
        if DEBUG:
            print('Asteroid l,r,t,b margins: %.2f, %.2f, %.2f, %.2f'%(lm, rm, tm, bm), flush=True)
        
        if(False):
            lim = self.ASTEROID_MIN_BORDER_MARGIN
            if (lm < lim and rm < lim or tm < lim and bm < lim):
                raise PositioningException('Asteroid too close: margins (l,r,t,b): %.2f, %.2f, %.2f, %.2f'%(lm, rm, tm, bm))
        else:
            lim = self.ASTEROID_MAX_SPAN
            if (1-lm-rm > lim or 1-tm-bm > lim):
                raise PositioningException('Asteroid too close: span (w,h): %.2f, %.2f'%(1-lm-rm, 1-tm-bm))
        
        # set too small regions to zero in scene image (stars)
        for i in range(n):
            if i>0 and i not in [p[0] for p in ast_parts]:
                sce_img[regs==i] = 0
        
        return tot_area
    
    
    # not in use, for a reason...
    def simple_adjusted_centroid(self, img):
        img = self.maybe_load_scene_image(img)
        
        cx, cy, pixels, hr_app, vr_app = self.get_image_centroid(img, is_scene=True, binary=True)
        
        solar_elongation, direction = self.system_model.solar_elongation()

        # percentage of object expected to be lit
        lit_pct = (1 - math.cos(solar_elongation)) / 2
        
        # if lit_pct -> 0% then correction -> inf 
        #   => need to cap corr when only a few pixels found
        # seems centroid moving is more susceptible than pixel count
        #   => different capping mechanisms
        
        # for pixel count adjusting 
        soft_cap_p = 1/50 # a tuning param (1/50 => corr < 51)
        se_corr_p = (1+soft_cap_p) / (lit_pct+soft_cap_p)
        
        # for centroid moving
        cap_s = 2.5  # a tuning param
        se_corr_s = min(1/lit_pct, cap_s)
        
        # size adjustment
        roughness_coef = 1.8   # a tuning param
        size_adj = se_corr_p ** roughness_coef
        
        # how many pixel spreads to move centroid
        mv_coef = 1 - se_corr_s
        
        # centroid correction based on solar elongation
        dx = hr_app * mv_coef * math.cos(direction)
        dy = vr_app * mv_coef * math.sin(direction)
        
        if not BATCH_MODE or DEBUG:
            # width & height estimate,
            # could be used for size? stars == danger?
            w = 2 * hr_app * (1+abs(mv_coef*math.cos(direction)))
            h = 2 * vr_app * (1+abs(mv_coef*math.sin(direction)))
            print(('centroid: [%.0f, %.0f, %.0f], '
                  +'correction: [%.0f, %.0f, %.1f], '
                  +'w & h: %.0f x %.0f') % (
                cx, cy, pixels, dx, dy, size_adj, w, h))        
            
            if False:
                img[-round(cy+dy),round(cx+dx),:] = np.array([0,0,255,128])
                cv2.imshow('centroid', img)
                cv2.waitKey()
        
        cx, cy, cs = round(cx+dx), round(cy+dy), pixels*size_adj

        # for cross-section currently assumes spherical object with mean rad 2km
        # TODO: somehow improve on this, now seems very crude
        r = math.sqrt(cs/math.pi)
        R = math.sqrt(system_model.asteroid.mean_cross_section/math.pi)
        angle = r/self._cam.height * math.radians(self._cam.y_fov)
        dist = R/1000 / math.tan(angle) # in km
        
        x_off, y_off = self._cam.calc_xy(cx, cy, -dist)
        
        return (x_off, y_off, -dist)
        

    def maybe_load_scene_image(self, sce_img, detect_asteroid=False, preproc=False):
        if isinstance(sce_img, str):
            sce_img = self.load_target_image(sce_img)

        if preproc:
            sce_img, self._bg_threshold = self.remove_background(sce_img)
            if detect_asteroid:
                self.detect_asteroid(sce_img)
            if DEBUG:
                sc = self.system_model.view_width / sce_img.shape[1]
                cv2.imshow('scene th=%d'%self._bg_threshold, cv2.resize(sce_img, None, fx=sc, fy=sc))
            
        return np.atleast_3d(sce_img)

    def get_image_centroid(self, img, is_scene=False, binary=False):
        ih, iw, cs = img.shape
        m = cv2.moments(img[:,:,0], binaryImage=binary)
        
        if np.isclose(m['m00'],0):
            if is_scene:
                raise PositioningException('No asteroid found')
            else:
                raise PositioningException('Algorithm failure: model moved out of view')
        
        # image centroid
        icx = m['m10']/m['m00']/iw*self._cam.width
        icy = m['m01']/m['m00']/ih*self._cam.height
        brightness = m['m00']/iw/ih*self._cam.width*self._cam.height
        
        # pixel spreads
        # used to model dimensions of asteroid parts visible in image
        hr = math.sqrt(m['mu20']/m['m00']) if m['mu20']>0 else 1
        vr = math.sqrt(m['mu02']/m['m00']) if m['mu02']>0 else 1
        
        return icx, icy, brightness, hr, vr class KeypointAlgo(AlgorithmBase):
    (
        ORB,
        AKAZE,
        SIFT,
        SURF,
    ) = range(4)

    BYTES_PER_FEATURE = {
        ORB: 32,
        AKAZE: 61,
        SIFT: 128,
        SURF: 64,
    }

    MAX_FEATURES = 1200         # upper limit of features to calculate from an image
    MAX_WORK_MEM = 0*1024       # in bytes, usable for both ref and scene features, default 512kB
    FDB_MAX_MEM = 512*1024      # in bytes per scene, default 192kB
    FDB_TOL = math.radians(12)  # features from db never more than FDB_TOL off
    FDB_REAL = True
    FDB_USE_ALL_FEATS = False

    MIN_FEATURES = 12  # fail if less inliers at the end

    # grid size in expected asteroid diameter fraction for discarding features too close to each other
    FEATURE_FILTERING_RELATIVE_GRID_SIZE = 0.015
    FEATURE_FILTERING_FALLBACK_GRID_SIZE = 2  # grid in pixels for use if don't know extent
    (FFS_NONE, FFS_SIMPLE_GRID, FFS_OVERLAPPING_GRID) = range(3)
    FEATURE_FILTERING_SCHEME = FFS_OVERLAPPING_GRID

    DISCARD_OFF_OBJECT_FEATURES = False

    # if want that mask radius is x when res 1024x1024 and ast dist 64km => coef = 64*x/1024
    MATCH_MASK_RADIUS = 0.15  # ratio to max asteroid diameter
    LOWE_METHOD_COEF = 0.85  # default 0.7 (0.8)  # 0.825 vs 0.8 => less fails, worse accuracy

    RANSAC_ITERATIONS = 20000  # default 100
    DEF_RANSAC_ERROR = 15  # default 8.0, for ORB: use half the error given here
    # (SOLVEPNP_ITERATIVE), SOLVEPNP_P3P, SOLVEPNP_AP3P, SOLVEPNP_EPNP, ?SOLVEPNP_UPNP, ?SOLVEPNP_DLS
    # AP3P seemed best, related paper: https://arxiv.org/pdf/1701.08237.pdf
    #  - related c-code: https://git.codingcafe.org/Mirrors/opencv/opencv/blob/master/modules/calib3d/src/ap3p.cpp
    RANSAC_KERNEL = cv2.SOLVEPNP_AP3P

    def __init__(self, system_model, render_engine, obj_idx, est_real_ast_orient=False):
        super(KeypointAlgo, self).__init__(system_model, render_engine, obj_idx)

        self.sm_noise = 0
        self._noise_lv = False
        self.extra_values = None
        self.est_real_ast_orient = est_real_ast_orient

        self._pause = DEBUG == 2
        self._shape_model_rng = None
        self._latest_detector = None
        self._ransac_err = None
        self._render_z = None

        self._fdb_helper = None
        self._fdb_feat = None
        self._fdb = None
        self._fdb_sc_ast_perms = None
        self._fdb_light_perms = None

        self.DEBUG_IMG_POSTFIX = 'k'    # if batch mode, save result image in a file ending like this

        self.SCENE_SCALE_STEP = 1.4142  # sqrt(2) scale scene image by this amount if fail
        self.MAX_SCENE_SCALE_STEPS = 5  # from mid range 64km to near range 16km (64/sqrt(2)**(5-1) => 16)

        self.RENDER_SHADOWS = True
        self.RENDER_TEXTURES = True


    def solve_pnp(self, orig_sce_img, outfile, feat=ORB, use_feature_db=False, adjust_sc_rot = False,
            add_noise=False, scale_cam_img=False, vary_scale=False, match_mask_params=None, verbose=1, **kwargs):

        # set max mem usable by reference features, scene features use rest of MAX_WORK_MEM
        ref_max_mem = KeypointAlgo.FDB_MAX_MEM if use_feature_db else KeypointAlgo.MAX_WORK_MEM/2
        sm = self.system_model
        self._ransac_err = KeypointAlgo.DEF_RANSAC_ERROR
        self._render_z = -sm.min_med_distance
        init_z = kwargs.get('init_z', self._render_z)
        ref_img_sc = min(1, self._render_z / init_z) * (sm.view_width if scale_cam_img else self._cam.width) / sm.view_width
        self.extra_values = None

        if outfile is not None:
            self.debug_filebase = outfile + (self.DEBUG_IMG_POSTFIX if isinstance(orig_sce_img, str) else '')

        if self.est_real_ast_orient:
            # so that can track rotation of 67P
            sm.reset_to_real_vals()

        if use_feature_db and self._fdb_helper is None:
            from visnav.algo.fdbgen import FeatureDatabaseGenerator
            self._fdb_helper = FeatureDatabaseGenerator(self.system_model, self.render_engine, self.obj_idx)

        # maybe load scene image
        if isinstance(orig_sce_img, str):
            orig_sce_img = self.load_target_image(orig_sce_img)

        if add_noise:
            self._shape_model_rng = np.max(np.ptp(sm.asteroid.real_shape_model.vertices, axis=0))

        self.timer = Stopwatch()
        self.timer.start()

        if use_feature_db:
            if KeypointAlgo.FDB_REAL:
                # find correct set of keypoints & descriptors from features db
                ref_desc, ref_kp_3d, ref_kp, ref_img = self._query_fdb(feat)
            else:
                # calculate on-the-fly exactly the same features that would be returned from a feature db
                ref_desc, ref_kp_3d, ref_kp, ref_img = self._fake_fdb(feat)
        else:
            # render model image
            ref_img, depth_result = self.render_ref_img(ref_img_sc)

            if False:
                # normalize ref_img to match sce_img
                ref_img = ImageProc.equalize_brightness(ref_img, orig_sce_img, percentile=99.999, image_gamma=1.8)

            if False:
                gamma = 1.0/1.8
                ref_img = ImageProc.adjust_gamma(ref_img, gamma)
                orig_sce_img = ImageProc.adjust_gamma(orig_sce_img, gamma)

            # get keypoints and descriptors
            ee = sm.pixel_extent(abs(self._render_z))
            ref_kp, ref_desc, self._latest_detector = KeypointAlgo.detect_features(ref_img, feat, maxmem=ref_max_mem,
                                                                                   max_feats=KeypointAlgo.MAX_FEATURES,
                                                                                   for_ref=True, expected_pixel_extent=ee)

        if BATCH_MODE and self.debug_filebase:
            # save start situation in log archive
            self.timer.stop()
            img1 = cv2.resize(orig_sce_img, (sm.view_width, sm.view_height))
            img2 = cv2.resize(ref_img, (sm.view_width, sm.view_height))
            cv2.imwrite(self.debug_filebase+'a.png', np.concatenate((img1, img2), axis=1))
            if DEBUG:
                cv2.imshow('compare', np.concatenate((img1, img2), axis=1))
            self.timer.start()

        # AKAZE, SIFT, SURF are truly scale invariant, couldnt get ORB to work as good
        vary_scale = vary_scale if feat==self.ORB else False
        
        if len(ref_kp) < KeypointAlgo.MIN_FEATURES:
            raise PositioningException('Too few (%d) reference features found' % (len(ref_kp),))

        ok = False
        for i in range(self.MAX_SCENE_SCALE_STEPS):
            try:
                # resize scene image if necessary
                sce_img_sc = (sm.view_width if scale_cam_img else self._cam.width)/self._cam.width/self.SCENE_SCALE_STEP**i
                if np.isclose(sce_img_sc, 1):
                    sce_img = orig_sce_img
                else:
                    sce_img = cv2.resize(orig_sce_img, None, fx=sce_img_sc, fy=sce_img_sc, interpolation=cv2.INTER_CUBIC)

                # detect features in scene image
                sce_max_mem = KeypointAlgo.MAX_WORK_MEM - (KeypointAlgo.BYTES_PER_FEATURE[feat] + 12)*len(ref_desc)
                ee = sm.pixel_extent(abs(match_mask_params[2])) if match_mask_params is not None else 0
                sce_kp, sce_desc, self._latest_detector = KeypointAlgo.detect_features(sce_img, feat, maxmem=sce_max_mem,
                                                                                       max_feats=KeypointAlgo.MAX_FEATURES,
                                                                                       expected_pixel_extent=ee)
                if len(sce_kp) < KeypointAlgo.MIN_FEATURES:
                    raise PositioningException('Too few (%d) scene features found' % (len(sce_kp),))

                # match descriptors
                try:
                    mask = None
                    if match_mask_params is not None:
                        mask = KeypointAlgo.calc_match_mask(sm, sce_kp, ref_kp, self._render_z,
                                                            sce_img_sc, ref_img_sc, *match_mask_params)
                    matches = KeypointAlgo.match_features(sce_desc, ref_desc, self._latest_detector.defaultNorm(),
                                                          mask=mask, method='brute')
                    error = None
                except PositioningException as e:
                    matches = []
                    error = e

                # debug by drawing matches
                if not BATCH_MODE or DEBUG:
                    print('matches: %s/%s'%(len(matches), min(len(sce_kp), len(ref_kp))), flush=True, end=", ")
                if verbose > 0:
                    self._draw_matches(sce_img, sce_img_sc, sce_kp, ref_img, ref_img_sc,
                                       ref_kp, matches, pause=False, show=DEBUG)

                if error is not None:
                    raise error

                # select matched scene feature image coordinates
                sce_kp_2d = np.array([tuple(np.divide(sce_kp[m.queryIdx].pt, sce_img_sc)) for m in matches], dtype='float')

                # prepare reference feature 3d coordinates (for only matched features)
                if use_feature_db:
                    ref_kp_3d = ref_kp_3d[[m.trainIdx for m in matches], :]
                    if add_noise:
                        # add noise to noiseless 3d ref points from fdb
                        self.timer.stop()
                        ref_kp_3d, self.sm_noise, _ = tools.points_with_noise(ref_kp_3d, only_z=True,
                                                                              noise_lv=SHAPE_MODEL_NOISE_LV[add_noise],
                                                                              max_rng=self._shape_model_rng)
                        self.timer.start()
                else:
                    # get feature 3d points using 3d model
                    ref_kp_3d = KeypointAlgo.inverse_project(sm, [ref_kp[m.trainIdx].pt for m in matches], depth_result,
                                                             self._render_z, ref_img_sc)

                if KeypointAlgo.DISCARD_OFF_OBJECT_FEATURES:
                    I = np.where(np.logical_not(np.isnan(ref_kp_3d[:, 0])))[0]
                    if len(I) < self.MIN_FEATURES:
                        raise PositioningException('Too few matches found')
                    sce_kp_2d = sce_kp_2d[I, :]
                    ref_kp_3d = ref_kp_3d[I, :]
                    matches = [matches[i] for i in I]

                # finally solve pnp with ransac
                rvec, tvec, inliers = KeypointAlgo.solve_pnp_ransac(sm, sce_kp_2d, ref_kp_3d, self._ransac_err)

                # debug by drawing inlier matches
                if verbose > 0:
                    self._draw_matches(sce_img, sce_img_sc, sce_kp, ref_img, ref_img_sc, ref_kp,
                                       [matches[i[0]] for i in inliers], label='c) inliers', pause=self._pause)

                inlier_count = self.count_inliers(sce_kp, ref_kp, matches, inliers)
                if DEBUG:
                    print('inliers: %s/%s, ' % (inlier_count, len(matches)), end='', flush=True)
                if inlier_count < KeypointAlgo.MIN_FEATURES:
                    raise PositioningException('RANSAC algorithm was left with too few inliers')

                # dont try again if found enough inliers
                ok = True
                break
            
            except PositioningException as e:
                if not vary_scale:
                    raise e
                # maybe try again using scaled down scene image
                
        if not ok:
            raise PositioningException('Not enough inliers even if tried scaling scene image down x%.1f'%(1/sce_img_sc))
        elif vary_scale:
            print('success at x%.1f'%(1/sce_img_sc))
        
        self.timer.stop()
        
        # set model params to solved pose & pos
        self._set_sc_from_ast_rot_and_trans(rvec, tvec, self.latest_discretization_err_q, rotate_sc=adjust_sc_rot)

        # debugging
        if not BATCH_MODE or DEBUG:
            rp_err = KeypointAlgo.reprojection_error(self._cam, sce_kp_2d, ref_kp_3d, inliers, rvec, tvec)
            sh_err = sm.calc_shift_err()

            print('repr-err: %.2f, rel-rot-err: %.2f°, dist-err: %.2f%%, lat-err: %.2f%%, shift-err: %.1fm'%(
                rp_err,
                math.degrees(sm.rel_rot_err()),
                sm.dist_pos_err()*100,
                sm.lat_pos_err()*100,
                sh_err*1000,
            ), flush=True)

        # save result image
        if BATCH_MODE and self.debug_filebase:
            # save result in log archive
            res_img = self.render(shadows=self.RENDER_SHADOWS)
            sce_img = cv2.resize(orig_sce_img, tuple(np.flipud(res_img.shape)))
            cv2.imwrite(self.debug_filebase+'d.png', np.concatenate((sce_img, res_img), axis=1))

    def count_inliers(self, sce_kp, ref_kp, matches, inliers, div=2):
        ref_i = [ref_kp[matches[i[0]].trainIdx] for i in inliers]
        sce_i = [sce_kp[matches[i[0]].queryIdx] for i in inliers]

        ref_count = len({(int(p.pt[0])//div, int(p.pt[1])//div) for p in ref_i})
        sce_count = len({(int(p.pt[0])//div, int(p.pt[1])//div) for p in sce_i})

        return min(ref_count, sce_count)

    def render_ref_img(self, ref_img_sc):
        sm = self.system_model
        orig_z = sm.z_off.value
        sm.z_off.value = self._render_z
        ref_img, depth = self.render(center=True, depth=True, shadows=self.RENDER_SHADOWS, textures=self.RENDER_TEXTURES,
                                     reflection=RenderEngine.REFLMOD_LUNAR_LAMBERT)
        sm.z_off.value = orig_z

        # scale to match scene image asteroid extent in pixels
        ref_img = cv2.resize(ref_img, None, fx=ref_img_sc, fy=ref_img_sc, interpolation=cv2.INTER_CUBIC)
        return ref_img, depth

    @classmethod
    def detect_features(cls, img, feat, maxmem, max_feats=MAX_FEATURES, for_ref=False, **kwargs):
        expected_pixel_extent = abs(kwargs.pop('expected_pixel_extent', 0))

        t_max_feats = max_feats
        if cls.FEATURE_FILTERING_SCHEME is not cls.FFS_NONE and feat == KeypointAlgo.ORB:
            # in opencv orb it's impossible to control detection threshold directly,
            # it's set based on target feature count, TODO: implement better
            t_max_feats = 30000

        detector, nfeats = cls.get_detector(feat, maxmem, t_max_feats, for_ref, **kwargs)
        nfeats = min(max_feats, nfeats)

        kp, dc = detector.detectAndCompute(img, None)
        if kp is None:
            return None

        if cls.FEATURE_FILTERING_RELATIVE_GRID_SIZE > 0 and expected_pixel_extent > 0:
            grid = cls.FEATURE_FILTERING_RELATIVE_GRID_SIZE * expected_pixel_extent
        else:
            grid = cls.FEATURE_FILTERING_FALLBACK_GRID_SIZE

        if cls.FEATURE_FILTERING_SCHEME is not cls.FFS_NONE and grid > 0:
            ng = img.shape[0] // grid * (img.shape[1] // grid + 1) + img.shape[1] // grid + 1
            offsets = [(0, 0)]
            if cls.FEATURE_FILTERING_SCHEME == cls.FFS_OVERLAPPING_GRID:
                offsets.append((grid // 2, grid // 2))
            groups = {}
            for i, p in enumerate(kp):
                for j, (xoff, yoff) in enumerate(offsets):
                    group = (p.pt[0] + xoff) // grid \
                            + ((p.pt[1]+yoff) // grid) * (img.shape[1] // grid + 1) \
                            + j * ng
                    if group not in groups or groups[group][0] < p.response:
                        groups[group] = (p.response, i)
            idxs = {i for r, i in groups.values()}
            kp = [kp[i] for i in idxs]
            dc = [dc[i] for i in idxs]

        idxs = np.argsort([-p.response for p in kp])
        return [kp[i] for i in idxs[:nfeats]], [dc[i] for i in idxs[:nfeats]], detector

    @staticmethod
    def get_detector(feat, maxmem, max_feats=MAX_FEATURES, for_ref=False, **kwargs):
        # extra bytes needed for keypoints (only coordinates (float32), the rest not used)
        if maxmem > 0:
            nb = 4 * (3 if for_ref else 2)
            bytes_per_feat = KeypointAlgo.BYTES_PER_FEATURE[feat] + nb
            nfeats = kwargs.pop('nfeats', np.clip(int(maxmem / bytes_per_feat), 0, max_feats))
        else:
            nfeats = max_feats

        if feat == KeypointAlgo.ORB:
            detector = KeypointAlgo.orb_detector(nfeats=nfeats, **kwargs)
        elif feat == KeypointAlgo.AKAZE:
            detector = KeypointAlgo.akaze_detector(nfeats=nfeats, **kwargs)
        elif feat == KeypointAlgo.SIFT:
            detector = KeypointAlgo.sift_detector(nfeats=nfeats, **kwargs)
        elif feat == KeypointAlgo.SURF:
            detector = KeypointAlgo.surf_detector(nfeats=nfeats, **kwargs)
        else:
            assert False, 'invalid feature: %s' % (feat,)

        return detector, nfeats

    @staticmethod
    def orb_detector(nfeats=1000):
        params = {
            'nfeatures':nfeats,         # default: 500
            'edgeThreshold':31,         # default: 31
            'fastThreshold':10,         # default: 20
            'firstLevel':0,             # always 0
            'nlevels':8,                # default: 8
            'patchSize':31,             # default: 31
            'scaleFactor':1.2,          # default: 1.2
            'scoreType':cv2.ORB_HARRIS_SCORE,  # default ORB_HARRIS_SCORE, other: ORB_FAST_SCORE
            'WTA_K':2,                  # default: 2
        }
        return cv2.ORB_create(**params)

    @staticmethod
    def akaze_detector(nfeats=1000):
        params = {
            'descriptor_type':cv2.AKAZE_DESCRIPTOR_MLDB, # default: cv2.AKAZE_DESCRIPTOR_MLDB
            'descriptor_channels':3,    # default: 3
            'descriptor_size':0,        # default: 0
            'diffusivity':cv2.KAZE_DIFF_CHARBONNIER, # default: cv2.KAZE_DIFF_PM_G2
            'threshold':0.00005,         # default: 0.001
            'nOctaves':4,               # default: 4
            'nOctaveLayers':4,          # default: 4
        }
        return cv2.AKAZE_create(**params)

    @staticmethod
    def sift_detector(nfeats=1000):
        params = {
            'nfeatures':nfeats,
            'nOctaveLayers':3,          # default: 3
            'contrastThreshold':0.01,   # default: 0.04
            'edgeThreshold':25,         # default: 10
            'sigma':1.6,                # default: 1.6
        }
        return cv2.xfeatures2d.SIFT_create(**params)

    @staticmethod
    def surf_detector(nfeats=1000):
        params = {
            'hessianThreshold':100.0,   # default: 100.0
            'nOctaves':4,               # default: 4
            'nOctaveLayers':3,          # default: 3
            'extended':False,           # default: False
            'upright':False,            # default: False
        }
        return cv2.xfeatures2d.SURF_create(**params)

    @staticmethod
    def calc_match_mask(sm, sce_kp, ref_kp, render_z, sce_img_sc, ref_img_sc,
                        ix_off, iy_off, ast_z, uncertainty_radius):

        n_sce_kp = np.tan((np.array([kp.pt for kp in sce_kp])
                        - np.array([ix_off + sm.cam.width/2, iy_off + sm.cam.height/2])*sce_img_sc
                   ) * math.radians(sm.cam.x_fov) / sm.cam.width).reshape((-1, 2)) * abs(ast_z)
        n_ref_kp = np.tan((np.array([kp.pt for kp in ref_kp])
                        - np.array([sm.view_width/2, sm.view_height/2])*ref_img_sc
                   ) * math.radians(sm.cam.x_fov) / sm.cam.width).reshape((-1, 2)) * abs(render_z)
        D = tools.distance_mx(n_sce_kp, n_ref_kp)

        match_radius = uncertainty_radius + KeypointAlgo.MATCH_MASK_RADIUS * sm.asteroid.max_radius * 2 / 1000
        return (D < match_radius).astype('uint8')

    @staticmethod
    def match_features(desc1, desc2, norm, mask=None, method='brute', symmetry_test=False, ratio_test=True):
        
        if desc1 is None or desc2 is None or len(desc1) < KeypointAlgo.MIN_FEATURES or len(desc2) < KeypointAlgo.MIN_FEATURES:
            raise PositioningException('Not enough features found')
        
        if method == 'flann':
            ss = norm != cv2.NORM_HAMMING

            FLANN_INDEX_LSH = 6     # for ORB, AKAZE
            FLANN_INDEX_KDTREE = 0  # for SIFT, SURF

            index_params = {
                'algorithm':    FLANN_INDEX_KDTREE if ss else FLANN_INDEX_LSH,
                'table_number': 6,       # 12
                'key_size':     12,      # 20
                'multi_probe_level': 1,  # 2
            }
            if ss:
                index_params['trees'] = 5

            search_params = {
                'checks': 100,
            }

            matcher = cv2.FlannBasedMatcher(index_params, search_params)
        elif method == 'brute':
            matcher = cv2.BFMatcher(norm, symmetry_test)
        else:
            assert False, 'unknown method %s' % method

        if ratio_test:
            matches = matcher.knnMatch(np.array(desc1), np.array(desc2), 2, mask=mask)
        else:
            matches = matcher.match(np.array(desc1), np.array(desc2), mask=mask)

        if len(matches) < KeypointAlgo.MIN_FEATURES:
            raise PositioningException('Not enough features matched')

        if ratio_test:
            # ratio test as per "Lowe's paper"
            matches = list(
                m[0]
                for m in matches
                if len(m) > 1 and m[0].distance < KeypointAlgo.LOWE_METHOD_COEF*m[1].distance
            )
            if len(matches) < KeypointAlgo.MIN_FEATURES:
                raise PositioningException('Too many features discarded')

        return matches

    
    def _draw_matches(self, img1, img1_sc, kp1, img2, img2_sc, kp2, matches, pause=True, show=True, label='b) matches'):
        self.timer.stop()
        
        matches = list([m] for m in matches)
        draw_params = {
#            matchColor: (88, 88, 88),
            'singlePointColor': (0, 0, 255),
#            'flags': cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS,
        }
        
        # scale keypoint positions
        for kp in kp1:
            kp.pt = tuple(np.divide(kp.pt, img1_sc*self._cam.width/self.system_model.view_width))
        for kp in kp2:
            kp.pt = tuple(np.divide(kp.pt, img2_sc))
        
        # scale image
        img1sc = cv2.cvtColor(cv2.resize(img1, None,
                            fx=1/img1_sc*self.system_model.view_width/self._cam.width,
                            fy=1/img1_sc*self.system_model.view_height/self._cam.height,
                            interpolation=cv2.INTER_CUBIC), cv2.COLOR_GRAY2RGB)
        img2sc = cv2.cvtColor(cv2.resize(img2, None,
                            fx=1/img2_sc,
                            fy=1/img2_sc,
                            interpolation=cv2.INTER_CUBIC), cv2.COLOR_GRAY2RGB)

        img3 = cv2.drawMatchesKnn(img1sc, kp1, img2sc, kp2, matches, None, **draw_params)

        # restore original keypoint positions
        for kp in kp1:
            kp.pt = tuple(np.multiply(kp.pt, img1_sc*self._cam.width/self.system_model.view_width))
        for kp in kp2:
            kp.pt = tuple(np.multiply(kp.pt, img2_sc))

        if BATCH_MODE and self.debug_filebase:
            cv2.imwrite(self.debug_filebase+label[:1]+'.png', img3)
        
        if show:
            cv2.imshow(label, img3)
        cv2.waitKey(0 if pause else 25)
        
        self.timer.start()

    @staticmethod
    def solve_pnp_ransac(sm, sce_kp_2d, ref_kp_3d, ransac_err=DEF_RANSAC_ERROR,
                         n_iter=RANSAC_ITERATIONS, kernel=RANSAC_KERNEL):
        
        # assuming no lens distortion
        dist_coeffs = None
        cam_mx = sm.cam.intrinsic_camera_mx()
        ref_kp_3d = np.reshape(ref_kp_3d, (len(ref_kp_3d), 1, 3))
        sce_kp_2d = np.reshape(sce_kp_2d, (len(sce_kp_2d), 1, 2))

        retval, rvec, tvec, inliers = cv2.solvePnPRansac(
                ref_kp_3d, sce_kp_2d, cam_mx, dist_coeffs,
                iterationsCount=n_iter,
                reprojectionError=ransac_err,
                flags=kernel)
        
        if not retval:
            raise PositioningException('RANSAC algorithm returned False')
        if len(inliers) >= KeypointAlgo.MIN_FEATURES and np.linalg.norm(tvec) > sm.max_distance * 1.1:
            # BUG in solvePnPRansac: sometimes estimates object to be very far even though enough good inliers
            # happens with all kernels, reprojectionErrors and iterationsCounts
            raise PositioningException('RANSAC estimated that asteroid at %s km' % (tvec.T,))

        return rvec, tvec, inliers

    @staticmethod
    def reprojection_error(cam, sce_kp_2d, ref_kp_3d, inliers, rvec, tvec):
        dist_coeffs = None
        cam_mx = cam.intrinsic_camera_mx()
        ref_kp_3d = np.reshape(ref_kp_3d, (len(ref_kp_3d), 1, 3))
        sce_kp_2d = np.reshape(sce_kp_2d, (len(sce_kp_2d), 1, 2))
        prj_kp_2d, _ = cv2.projectPoints(ref_kp_3d, rvec, tvec, cam_mx, dist_coeffs)
        return np.sqrt(np.mean((sce_kp_2d[inliers] - prj_kp_2d[inliers])**2))

    @staticmethod
    def inverse_project(system_model, points_2d, depths, render_z, img_sc, max_dist=30):
        cam = system_model.cam
        sc = img_sc * system_model.view_width/cam.width
        max_val = system_model.max_distance-3
        discard_bg = KeypointAlgo.DISCARD_OFF_OBJECT_FEATURES
        idxs = None if discard_bg else tools.foreground_idxs(depths, max_val=max_val)

        def invproj(xi, yi):
            z = -tools.interp2(depths, xi/img_sc, yi/img_sc, idxs=idxs, max_val=max_val, max_dist=max_dist,
                               discard_bg=discard_bg)
            x, y = cam.calc_xy(xi/sc, yi/sc, z)
            return x, -y, -(z-render_z) # same as rotate using cv2gl_q
        
        points_3d = np.array([invproj(pt[0], pt[1]) for pt in points_2d], dtype='float32')
        return points_3d
    
    def _set_sc_from_ast_rot_and_trans(self, rvec, tvec, discretization_err_q, rotate_sc=False):
        sm = self.system_model
        
        # rotate to gl frame from opencv camera frame
        gl2cv_q = sm.frm_conv_q(sm.OPENGL_FRAME, sm.OPENCV_FRAME)
        new_sc_pos = tools.q_times_v(gl2cv_q, tvec)

        # camera rotation in opencv frame
        cv_cam_delta_q = tools.angleaxis_to_q(rvec)

        # solvePnPRansac has some bug that apparently randomly gives 180deg wrong answer
        if new_sc_pos[2]>0:
            tpos = -new_sc_pos
            tdelta_q = cv_cam_delta_q * tools.ypr_to_q(0,math.pi,0)
            # print('Bug with solvePnPRansac, correcting:\n\t%s => %s\n\t%s => %s'%(
            #     new_sc_pos, tpos, tools.q_to_ypr(cv_cam_delta_q), tools.q_to_ypr(tdelta_q)))
            new_sc_pos = tpos
            cv_cam_delta_q = tdelta_q
        
        sm.spacecraft_pos = new_sc_pos
        
        err_q = discretization_err_q or np.quaternion(1, 0, 0, 0)
        if rotate_sc:
            sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME)
            sc_delta_q = err_q * sc2cv_q * cv_cam_delta_q.conj() * sc2cv_q.conj()
            sm.rotate_spacecraft(sc_delta_q)  # TODO: check that rotation ok, changed from sc_q = sc_q * dq to sc_q = dq * sc_q
        else:
            sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME)
            sc_q = sm.spacecraft_q

            frame_q = sc_q * err_q * sc2cv_q
            ast_delta_q = frame_q * cv_cam_delta_q * frame_q.conj()
            
            err_corr_q = sc_q * err_q.conj() * sc_q.conj()
            ast_q0 = sm.asteroid_q
            sm.rotate_asteroid(err_corr_q * ast_delta_q)

            if self.est_real_ast_orient:
                # so that can track rotation of 67P
                ast_q = sm.asteroid_q
                err_deg = math.degrees(tools.angle_between_q(ast_q0, ast_q))
                self.extra_values = list(quaternion.as_float_array(ast_q)) + [sm.time.value, err_deg]

    def _fake_fdb(self, feat):
        if self._fdb_sc_ast_perms is None or self._fdb_light_perms is None:
            a, b = self._fdb_helper.calc_mesh(KeypointAlgo.FDB_TOL)
            self._fdb_helper.set_mesh(a, b)
            self._fdb_sc_ast_perms, self._fdb_light_perms = a, b

        # get closest scene in fdb
        i1, i2, d_sc_ast_q, d_light_v, err_q, err_angle = self._fdb_helper.closest_scene()

        sc2gl_q = self.system_model.frm_conv_q(self.system_model.SPACECRAFT_FRAME, self.system_model.OPENGL_FRAME)
        self.latest_discretization_err_q = sc2gl_q * err_q * sc2gl_q.conj()

        self.timer.stop()

        # render it and extract features
        ref_img, depth = self._fdb_helper.render_scene(i1, i2)
        ref_kp, ref_desc, detector = KeypointAlgo.detect_features(ref_img, feat, KeypointAlgo.FDB_MAX_MEM,
                                                                  self._fdb_helper.MAX_FEATURES, for_ref=True)

        # get 3d coordinates
        ref_img_sc = self._cam.width / self.system_model.view_width
        ref_kp_2d = np.array([p.pt for p in ref_kp], dtype='float32')
        ref_kp_3d = KeypointAlgo.inverse_project(self.system_model, ref_kp_2d, depth, self._render_z, ref_img_sc)

        if KeypointAlgo.FDB_USE_ALL_FEATS:
            self.timer.start()
            return ref_desc, ref_kp_3d, ref_kp, ref_img

        # get closest neighbours
        visit = self._fdb_helper.get_neighbours(KeypointAlgo.FDB_TOL, i1, i2)
        matched_feats = np.zeros((len(ref_kp_3d),), dtype='bool')
        for i1, i2, j1, j2 in visit:
            try:
                # render neighbour, get features
                n_img, _ = self._fdb_helper.render_scene(j1, j2, get_depth=False)
                n_kp, n_desc, _ = KeypointAlgo.detect_features(n_img, feat, KeypointAlgo.FDB_MAX_MEM,
                                                               self._fdb_helper.MAX_FEATURES, for_ref=True)

                # match features
                matches = KeypointAlgo.match_features(n_desc, ref_desc, detector.defaultNorm(), method='brute')

                # solve pnp with ransac
                kp_3d = ref_kp_3d[[m.trainIdx for m in matches], :]
                kp_2d = np.array([n_kp[m.queryIdx].pt for m in matches])
                rvec, tvec, inliers = KeypointAlgo.solve_pnp_ransac(self.system_model, kp_2d, kp_3d, self._ransac_err)

                # check that solution is correct
                ok, err1, err2 = self._fdb_helper.calc_err(rvec, tvec, j1, i1, warn=len(inliers) > 30)

                if ok:
                    # update matches
                    matched_feats[[matches[i[0]].trainIdx for i in inliers]] = True

            except PositioningException:
                pass

        ref_desc = np.array(ref_desc)[matched_feats]
        ref_kp_3d = ref_kp_3d[matched_feats]
        ref_kp = [k for i, k in enumerate(ref_kp) if matched_feats[i]]

        self.timer.start()
        return ref_desc, ref_kp_3d, ref_kp, ref_img

    def _query_fdb(self, feat):
        self._ensure_fdb_loaded(feat)

        i1, i2, d_sc_ast_q, d_light_v, err_q, err_angle = self._fdb_helper.closest_scene()

        sc2gl_q = self.system_model.frm_conv_q(self.system_model.SPACECRAFT_FRAME, self.system_model.OPENGL_FRAME)
        self.latest_discretization_err_q = sc2gl_q * err_q * sc2gl_q.conj()

        if KeypointAlgo.FDB_USE_ALL_FEATS:
            # all features
            nf = self._fdb[4][i1, i2]
            ref_desc = self._fdb[0][i1, i2, 0:nf, :]
            ref_kp_3d = self._fdb[2][i1, i2, 0:nf, :]
        else:
            # only features that have matched with a neighbor mesh node
            ref_desc = self._fdb[0][i1, i2, self._fdb[3][i1, i2, :], :]
            ref_kp_3d = self._fdb[2][i1, i2, self._fdb[3][i1, i2, :], :]

        self.timer.stop()

        # render feature db scene for plotting
        ref_img, depth = self._fdb_helper.render_scene(i1, i2)
        # pos = (0, 0, -self.system_model.min_med_distance)
        # render_engine.render(self.obj_idx, pos, d_sc_ast_q, d_light_v)
        # ref_img = cv2.cvtColor(ref_img, cv2.COLOR_RGB2GRAY)

        # construct necessary parts of ref_kp for plotting
        ref_kp = [cv2.KeyPoint(*self._cam.calc_img_xy(x, -y, -z+self._render_z), 1) for x, y, z in ref_kp_3d]

        self.timer.start()
        return ref_desc, ref_kp_3d, ref_kp, ref_img

    def _ensure_fdb_loaded(self, feat):
        self.timer.stop()
        if self._fdb is None or self._fdb_feat != feat:
            fname = self._fdb_helper.fdb_fname(feat)
            try:
                status, self._fdb_sc_ast_perms, self._fdb_light_perms, self._fdb = self._fdb_helper.load_fdb(fname)
                self._fdb_helper.set_mesh(self._fdb_sc_ast_perms, self._fdb_light_perms)
                self._fdb_feat = feat
            except (FileNotFoundError, EOFError):
                assert False, 'Couldn\'t find feature db file %s' % fname
        self.timer.start() class MixedAlgo(AlgorithmBase):
    EXPECTED_LATERAL_ERR_ANGLE_SD = 0.30      # in deg
    EXPECTED_RELATIVE_DISTANCE_ERR_SD = 0.10  # 0.1 == 10% err

    def __init__(self, centroid, keypoint, **kwargs):
        super(MixedAlgo, self).__init__(centroid.system_model, centroid.render_engine, centroid.obj_idx)
        self._centroid = centroid
        self._keypoint = keypoint

    def run(self, sce_img, outfile, **kwargs):
        centroid_result = None
        keypoint_ok = False
        try:
            if True:
                self._centroid.adjust_iteratively(sce_img, None, **kwargs)
                sc_r1 = self.system_model.spacecraft_rot
                ast_r1 = self.system_model.asteroid_axis
                rel_q1 = self.system_model.sc_asteroid_rel_q()
                centroid_result = self.system_model.spacecraft_pos
            else:
                centroid_result = self.system_model.real_spacecraft_pos
            #kwargs['init_z'] = centroid_result[2]

            x_off, y_off = self._cam.calc_img_xy(*centroid_result)
            uncertainty_radius = math.tan(math.radians(MixedAlgo.EXPECTED_LATERAL_ERR_ANGLE_SD) * 2) \
                                 * abs(centroid_result[2]) * (1 + MixedAlgo.EXPECTED_RELATIVE_DISTANCE_ERR_SD * 2)
            kwargs['match_mask_params'] = (
                x_off-self._cam.width/2,
                y_off-self._cam.height/2,
                centroid_result[2],
                uncertainty_radius,
            )
            d1 = np.linalg.norm(centroid_result)
        except PositioningException as e:
            if str(e) == 'No asteroid found':
                raise e
            elif not 'Asteroid too close' in str(e) and DEBUG:
                print('Centroid algo failed with: %s'%(e,))

        centroid_ok = centroid_result is not None
        fallback = centroid_ok and kwargs.get('centroid_fallback', False) \
                   and self.system_model.max_distance > d1 > self.system_model.min_med_distance
        try:
            self._keypoint.solve_pnp(sce_img, outfile, **kwargs)

            d2 = np.linalg.norm(self.system_model.spacecraft_pos)
            rel_q2 = self.system_model.sc_asteroid_rel_q()
            d_ang = abs(tools.wrap_rads(tools.angle_between_q(rel_q1, rel_q2))) if fallback else None
            if fallback and (d2 > d1 * 1.2 or d_ang > math.radians(20)):
                # if keypoint res distance significantly larger than from centroid method, override
                # also, if orientation significantly different from initial, override
                self.system_model.spacecraft_pos = centroid_result
                self.system_model.spacecraft_rot = sc_r1
                self.system_model.asteroid_axis = ast_r1
            elif fallback and d2 > self.system_model.max_med_distance:
                # if distance more than max medum distance, assume orientation result is wrong
                self.system_model.spacecraft_rot = sc_r1
                self.system_model.asteroid_axis = ast_r1
            else:
                keypoint_ok = True
        except PositioningException as e:
            if fallback:
                self.system_model.spacecraft_pos = centroid_result
                self.system_model.spacecraft_rot = sc_r1
                self.system_model.asteroid_axis = ast_r1
                if DEBUG:
                    print('Using centroid result as keypoint algo failed: %s'%(e,))
            else:
                raise e

        return (centroid_ok or keypoint_ok, keypoint_ok) class PositioningException(Exception):
    pass def MTL(filename):
    def __init__(self, fname=None, data=None):
    def from_file(self, fname, swapyz=False):
    def _face_massage(faces):
    def from_dict(self, data):
    def as_dict(self):
    def recalc_norms(self):
    def pack_all(self):
    def pack_simple(self):
    def texture_to_vertex_map(self):
    def export_smooth_faces(self):
    def export_angular_faces(self):
    def load_texture(self, normalize=True):
    def tex(self):
    def tex(self, new_tex):
class ShapeModel: def load_image_meta(src, sm):

import sys
import math
import threading
import _thread
import numpy as np
import cv2
import visnav.settings
            import cProfile
from PyQt5.QtGui import QColor, QSurfaceFormat, QOpenGLVersionProfile
from PyQt5.QtCore import (pyqtSignal, QPoint, QSize, Qt, QBuffer, QIODevice,
        QCoreApplication)
from PyQt5.QtWidgets import (QApplication, QHBoxLayout, QVBoxLayout,
        QOpenGLWidget, QSlider, QPushButton, QWidget)
from visnav.algo.base import AlgorithmBase
from visnav.missions.rosetta import RosettaSystemModel
from visnav.render.render import RenderEngine
from visnav.settings import *
from visnav.algo import tools
from visnav.algo.model import SystemModel
from visnav.algo.image import ImageProc
from visnav.algo.phasecorr import PhaseCorrelationAlgo
from visnav.algo.centroid import CentroidAlgo
from visnav.algo.keypoint import KeypointAlgo
from visnav.algo.mixed import MixedAlgo
from visnav.algo.tools import PositioningException
from visnav.iotools import objloader
from visnav.iotools import lblloader



#from OpenGL.GL.images import glReadPixels

visnav.settings.BATCH_MODE = False




class MainThread(threading.Thread):
    def __init__(self, counter):
        super(MainThread, self).__init__()
        self.threadID = counter
        self.name = 'main-thread-%s'%counter
        self.counter = counter
        self.ready = threading.Event()
        self.window = None
        
    def run(self):
        sys.tracebacklimit = 10
        self.app = QApplication(sys.argv)
        self.window = Window()
        self.window.show()
        self.ready.set()
        self.app.exec_()
        
    def wait_until_ready(self):
        self.ready.wait()



class Window(QWidget):
    tsRun = pyqtSignal(tuple)
    
    def __init__(self):
        super(Window, self).__init__()
        sm = RosettaSystemModel(rosetta_batch='mtp006')
        sample_img = 'ROS_CAM1_20140822T020718'
        sm.asteroid.sample_image_file = os.path.join(sm.asteroid.image_db_path, sample_img + '_P.png')
        sm.asteroid.sample_image_meta_file = os.path.join(sm.asteroid.image_db_path, sample_img + '.LBL')

        self.systemModel = sm
        self.glWidget = GLWidget(self.systemModel, parent=self)
        self.closing = []
        
        # so that can run algorithms as a batch from different thread
        def tsRunHandler(f):
            fun, args, kwargs = f[0], f[1] or [], f[2] or {}
            self.tsRunResult = fun(*args, **kwargs)
        self.tsRun.connect(tsRunHandler)
        self.tsRunResult = None, float('nan')
        
        self.sliders = dict(
            (n, self.slider(p))
            for n, p in self.systemModel.get_params(all=True)
        )
        
        topLayout = QHBoxLayout()
        topLayout.addWidget(self.glWidget)
        topLayout.addWidget(self.sliders['x_off'])
        topLayout.addWidget(self.sliders['y_off'])
        topLayout.addWidget(self.sliders['z_off'])
        topLayout.addWidget(self.sliders['x_rot'])
        topLayout.addWidget(self.sliders['y_rot'])
        topLayout.addWidget(self.sliders['z_rot'])
        topLayout.addWidget(self.sliders['ast_x_rot'])
        topLayout.addWidget(self.sliders['ast_y_rot'])
        topLayout.addWidget(self.sliders['ast_z_rot'])
        topLayout.addWidget(self.sliders['time'])

        bottomLayout = QHBoxLayout()
        render_engine = RenderEngine(sm.view_width, sm.view_width)
        obj_idx = render_engine.load_object(os.path.join(DATA_DIR, '67p-4k.obj'))
        self.phasecorr = PhaseCorrelationAlgo(sm, render_engine, obj_idx)
        self.keypoint = KeypointAlgo(sm, render_engine, obj_idx)
        self.centroid = CentroidAlgo(sm, render_engine, obj_idx)
        self.mixed = MixedAlgo(self.centroid, self.keypoint)
        
        self.buttons = dict(
            (m.lower(), self.optbutton(m, bottomLayout))
            for m in (
                'Simplex',
#                'Powell',
                'COBYLA',
#                'CG',
#                'BFGS',
                'Anneal',
                'Brute',
            )
        )
        
        self.infobtn = QPushButton('Info', self)
        self.infobtn.clicked.connect(lambda: self.printInfo())
        bottomLayout.addWidget(self.infobtn)
        
#        self.zoombtn = QPushButton('+', self)
#        self.zoombtn.clicked.connect(lambda: self.glWidget.setImageZoomAndResolution(
#                im_xoff=300, im_yoff=180, 
#                im_width=512, im_height=512, im_scale=1))
#        bottomLayout.addWidget(self.zoombtn)
#        
#        self.defviewbtn = QPushButton('=', self)
#        self.defviewbtn.clicked.connect(lambda: self.glWidget.setImageZoomAndResolution(
#                im_xoff=0, im_yoff=0, 
#                im_width=1024, im_height=1024, im_scale=0.5))
#        bottomLayout.addWidget(self.defviewbtn)
        
        def testfun1():
            try:
                self.centroid.adjust_iteratively(self.glWidget.image_file, LOG_DIR)

            except PositioningException as e: