def make_default_system():
    '''helper function to generate an instance of MultiCameraSystem'''
    lookat = np.array( (0.0, 0.0, 0.0))

    center1 = np.array( (0.0, 0.0, 0.9) )
    distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] )
    cam1 = CameraModel.load_camera_simple(name='cam1',
                                          fov_x_degrees=90,
                                          eye=center1,
                                          lookat=lookat,
                                          distortion_coefficients=distortion1,
                                          )

    center2 = np.array( (0.5, -0.8, 0.0) )
    cam2 = CameraModel.load_camera_simple(name='cam2',
                                          fov_x_degrees=90,
                                          eye=center2,
                                          lookat=lookat,
                                          )

    center3 = np.array( (0.5, 0.5, 0.0) )
    cam3 = CameraModel.load_camera_simple(name='cam3',
                                          fov_x_degrees=90,
                                          eye=center3,
                                          lookat=lookat,
                                          )

    system = MultiCameraSystem([cam1,cam2,cam3])
    return system

def make_default_system(with_separate_distorions=False):
    """helper function to generate an instance of MultiCameraSystem"""
    if with_separate_distorions:
        raise NotImplementedError
    camxs = np.linspace(-2, 2, 3)
    camzs = np.linspace(-2, 2, 3).tolist()
    camzs.pop(1)
    cameras = []
    lookat = np.array((0.0, 0, 0))
    up = np.array((0.0, 0, 1))

    for enum, camx in enumerate(camxs):
        center = np.array((camx, -5, 0))
        name = "camx_%d" % (enum + 1,)
        cam = CameraModel.load_camera_simple(name=name, fov_x_degrees=45, eye=center, lookat=lookat, up=up)
        cameras.append(cam)

    for enum, camz in enumerate(camzs):
        center = np.array((0, -5, camz))
        name = "camz_%d" % (enum + 1,)
        cam = CameraModel.load_camera_simple(name=name, fov_x_degrees=45, eye=center, lookat=lookat, up=up)
        cameras.append(cam)

    system = MultiCameraSystem(cameras)
    return system

def test_equals():
    system = make_default_system()
    assert system != 1234

    system2 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)])
    system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(3)])
    assert system2 != system3

    system4 = make_default_system()
    d = system4.to_dict()
    d['camera_system'][0]['width'] += 1
    system5 = MultiCameraSystem.from_dict( d )
    assert system4 != system5

def test_align():
    system1 = make_default_system()
    system2 = system1.get_aligned_copy( system1 ) # This should be a no-op.
    assert system1==system2

    system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)])
    nose.tools.assert_raises(ValueError, system3.get_aligned_copy, system1)

def check_flip(distortion=False):
    if distortion:
        d = [0.1, 0.2, 0.3, 0.4, 0.5]
    else:
        d = None

    # build camera
    center_expected = np.array( [10, 5, 20] )
    lookat_expected = center_expected + np.array( [1, 2, 0] )
    up_expected     = np.array( [0,  0,  1] )

    width, height = 640, 480

    M = np.array( [[ 300.0,     0,  321, 0],
                      [ 0,     298.0,  240, 0],
                      [ 0,         0,   1,  0]])
    cam1 = CameraModel.load_camera_from_M( M, width=width, height=height,
                                              distortion_coefficients=d )
    cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected)
    del cam1

    pts = np.array([lookat_expected,
                    lookat_expected+up_expected,
                    [1,2,3],
                    [4,5,6]])
    pix_actual = cam.project_3d_to_pixel( pts )

    # Flipped camera gives same 3D->2D transform but different look direction.
    cf = cam.get_flipped_camera()
    assert not np.allclose( cam.get_lookat(), cf.get_lookat() )

    pix_actual_flipped = cf.project_3d_to_pixel( pts )
    assert np.allclose( pix_actual,      pix_actual_flipped )

def check_built_from_M(cam_opts):
    """check that M is preserved in load_camera_from_M() factory"""
    cam_orig = _build_test_camera(**cam_opts)
    if cam_orig.is_distorted_and_skewed():
        raise SkipTest('do not expect that skewed camera passes this test')
    M_orig = cam_orig.M
    cam = CameraModel.load_camera_from_M( M_orig )
    assert np.allclose( cam.M, M_orig)

def test_equality():
    center = np.array( (0, 0.0, 5) )
    lookat = center + np.array( (0,1,0))
    cam_apple1 = CameraModel.load_camera_simple(fov_x_degrees=90,
                                                eye=center,
                                                lookat=lookat,
                                                name='apple')
    cam_apple2 = CameraModel.load_camera_simple(fov_x_degrees=90,
                                                eye=center,
                                                lookat=lookat,
                                                name='apple')
    cam_orange = CameraModel.load_camera_simple(fov_x_degrees=30,
                                                eye=center,
                                                lookat=lookat,
                                                name='orange')
    assert cam_apple1==cam_apple2
    assert cam_apple1!=cam_orange
    assert not cam_apple1==cam_orange

def check_roundtrip_ros_tf(cam_opts):
    cam1 = _build_test_camera(**cam_opts)
    translation, rotation = cam1.get_ROS_tf()
    i = cam1.get_intrinsics_as_bunch()
    cam2 = CameraModel.load_camera_from_ROS_tf( translation=translation,
                                                rotation=rotation,
                                                intrinsics = i,
                                                name = cam1.name)
    assert cam1==cam2

def test_getters():
    system1 = make_default_system()
    d = system1.to_dict()
    names1 = list(system1.get_camera_dict().keys())
    names2 = [cd['name'] for cd in d['camera_system']]
    assert set(names1)==set(names2)

    for idx in range(len(names1)):
        cam1 = system1.get_camera( names1[idx] )
        cam2 = CameraModel.from_dict(d['camera_system'][idx])
        assert cam1==cam2

def check_align(cam_opts):

    cam_orig = _build_test_camera(**cam_opts)
    M_orig = cam_orig.M
    cam_orig = CameraModel.load_camera_from_M( M_orig )
    R1 = np.eye(3)
    R2 = np.zeros((3,3))
    R2[0,1] = 1
    R2[1,0] = 1
    R2[2,2] = -1
    t1 = np.array( (0.0, 0.0, 0.0) )
    t2 = np.array( (0.0, 0.0, 0.1) )
    t3 = np.array( (0.1, 0.0, 0.0) )
    for s in [1.0, 2.0]:
        for R in [R1, R2]:
            for t in [t1, t2, t3]:
                cam_actual = cam_orig.get_aligned_camera( s, R, t )
                M_expected = mcsc_align.align_M( s,R,t, M_orig )
                cam_expected = CameraModel.load_camera_from_M( M_expected )
                assert cam_actual==cam_expected

 def calc_mean_reproj_error(self,msg):
     ros_cam = CameraModel._from_parts(intrinsics=msg)
     all_ims = []
     for imd in self.db:
         ros_pix  = ros_cam.project_3d_to_pixel(imd['cc'], distorted=True)
         d = (ros_pix-imd['pix'])**2
         drows = np.sqrt(np.sum(d, axis=1))
         mean_d = np.mean(drows)
         all_ims.append(mean_d)
     mean_err = np.mean(all_ims)
     return mean_err

def test_simple_projection():

    # get some 3D points
    pts_3d = _build_points_3d()

    if DRAW:
        fig = plt.figure(figsize=(8,12))
        ax1 = fig.add_subplot(3,1,1, projection='3d')
        ax1.scatter( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2])
        ax1.set_xlabel('X')
        ax1.set_ylabel('Y')
        ax1.set_zlabel('Z')

    # build a camera calibration matrix
    focal_length = 1200
    width, height = 640,480
    R = np.eye(3) # look at +Z
    c = np.array( (9.99, 19.99, 20) )
    M = make_M( focal_length, width, height, R, c)['M']

    # now, project these 3D points into our image plane
    pts_3d_H = np.vstack( (pts_3d.T, np.ones( (1,len(pts_3d))))) # make homog.
    undist_rst_simple = np.dot(M, pts_3d_H) # multiply
    undist_simple = undist_rst_simple[:2,:]/undist_rst_simple[2,:] # project

    if DRAW:
        ax2 = fig.add_subplot(3,1,2)
        ax2.plot( undist_simple[0,:], undist_simple[1,:], 'b.')
        ax2.set_xlim(0,width)
        ax2.set_ylim(height,0)
        ax2.set_title('matrix multiply')

    # build a camera model from our M and project onto image plane
    cam = CameraModel.load_camera_from_M( M, width=width, height=height )
    undist_full = cam.project_3d_to_pixel(pts_3d).T

    if DRAW:
        plot_camera( ax1, cam, scale=10, axes_size=5.0 )
        sz = 20
        x = 5
        y = 8
        z = 19
        ax1.auto_scale_xyz( [x,x+sz], [y,y+sz], [z,z+sz] )

        ax3 = fig.add_subplot(3,1,3)
        ax3.plot( undist_full[0,:], undist_full[1,:], 'b.')
        ax3.set_xlim(0,width)
        ax3.set_ylim(height,0)
        ax3.set_title('pymvg')

    if DRAW:
        plt.show()

    assert np.allclose( undist_full, undist_simple )

    def new_data(self):
        with self._lock:
            if (self.translation is None or
                self.rotation is None or
                self.intrinsics is None):
                return
            newcam = CameraModel.load_camera_from_ROS_tf( translation=self.translation,
                                                          rotation=self.rotation,
                                                          intrinsics=self.intrinsics,
                                                          name=self.get_frame_id(),
                                                          )
        self.cam = newcam

        self.draw()

def test_lookat():

    dist = 5.0

    # build camera
    center_expected = np.array( [10, 5, 20] )
    lookat_expected = center_expected + np.array( [dist, 0, 0] ) # looking in +X
    up_expected     = np.array( [0,  0,  1] )

    f = 300.0 # focal length
    width, height = 640, 480
    cx, cy = width/2.0, height/2.0

    M = np.array( [[ f, 0, cx, 0],
                      [ 0, f, cy, 0],
                      [ 0, 0,   1, 0]])
    cam1 = CameraModel.load_camera_from_M( M, width=width, height=height)
    cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected)
    del cam1

    # check that the extrinsic parameters were what we expected
    (center_actual,lookat_actual,up_actual) = cam.get_view()

    lookdir_expected = normalize( lookat_expected - center_expected )
    lookdir_actual   = normalize( lookat_actual   - center_actual   )

    assert np.allclose( center_actual,  center_expected  )
    assert np.allclose( lookdir_actual, lookdir_expected )
    assert np.allclose( up_actual,      up_expected      )

    # check that the extrinsics work as expected
    pts = np.array([lookat_expected,
                    lookat_expected+up_expected])
    eye_actual = cam.project_3d_to_camera_frame( pts )

    eye_expected = [[0, 0, dist], # camera looks at +Z
                    [0,-1, dist], # with -Y as up
                    ]
    assert np.allclose( eye_actual,      eye_expected      )

    # now check some basics of the projection
    pix_actual = cam.project_3d_to_pixel( pts )

    pix_expected = [[cx,cy], # center pixel on the camera
                    [cx,cy-(f/dist)]]
    assert np.allclose( pix_actual,      pix_expected      )

def check_distortion_yamlfile_roundtrip(cam_opts):
    """check that roundtrip of camera model to/from a yaml file works"""
    cam = _build_test_camera(**cam_opts)
    fname = tempfile.mktemp(suffix='.yaml')
    cam.save_intrinsics_to_yamlfile(fname)
    try:
        cam2 = CameraModel.load_camera_from_file( fname, extrinsics_required=False )
    finally:
        os.unlink(fname)

    distorted = np.array( [[100.0,100],
                           [100,200],
                           [100,300],
                           [100,400]] )
    orig_undistorted = cam.undistort( distorted )
    reloaded_undistorted = cam2.undistort( distorted )
    assert np.allclose( orig_undistorted, reloaded_undistorted )

    def generate_camera(self):
        (width,height)=(self.width,self.height)=(640,480)
        center = 1,2,3
        rot_axis = np.array((4,5,6.7))
        rot_axis = rot_axis / np.sum(rot_axis**2)
        rquat = tf.transformations.quaternion_about_axis(0.1, (rot_axis.tolist()))
        rmat,_ = get_rotation_matrix_and_quaternion(rquat)

        parts = make_M( 1234.56, width, height,
                        rmat, center)

        if self.use_distortion:
            dist = [-0.4, .2, 0, 0, 0]
        else:
            dist = [0, 0, 0, 0, 0]

        self.cam = CameraModel.load_camera_from_M(parts['M'],
                                                  width=width,height=height,
                                                  distortion_coefficients=dist)

def build_example_system(n=6,z=5.0):
    base = CameraModel.load_camera_default()

    x = np.linspace(0, 2*n, n)
    theta = np.linspace(0, 2*np.pi, n)
    cams = []
    for i in range(n):
        # cameras are spaced parallel to the x axis
        center = np.array( (x[i], 0.0, z) )

        # cameras are looking at +y
        lookat = center + np.array( (0,1,0))

        # camera up direction rotates around the y axis
        up = -np.sin(theta[i]), 0, np.cos(theta[i])

        cam = base.get_view_camera(center,lookat,up)
        cam.name = 'theta: %.0f'%( np.degrees(theta[i]) )
        cams.append(cam)

    system = MultiCameraSystem(cams)
    return system

def test_problem_M():
    """check a particular M which previously caused problems"""
    # This M (found by the DLT method) was causing me problems.
    d = {'width': 848,
         'name': 'camera',
         'height': 480}
    M =  np.array([[ -1.70677031e+03,  -4.10373295e+03,  -3.88568028e+02, 6.89034515e+02],
                   [ -6.19019195e+02,  -1.01292091e+03,  -2.67534989e+03, 4.51847857e+02],
                   [ -4.52548832e+00,  -3.78900498e+00,  -7.35860226e-01, 1.00000000e+00]])
    cam = CameraModel.load_camera_from_M( M, **d)

    #assert np.allclose( cam.M, M) # we don't expect this since the intrinsic matrix may not be scaled

    verts = np.array([[ 0.042306,  0.015338,  0.036328, 1.0],
                      [ 0.03323,   0.030344,  0.041542, 1.0],
                      [ 0.036396,  0.026464,  0.052408, 1.0]])

    actual = cam.project_3d_to_pixel(verts[:,:3])

    expectedh = np.dot( M, verts.T )
    expected = (expectedh[:2]/expectedh[2]).T
    assert np.allclose( expected, actual )

def check_bagfile_roundtrip(cam_opts):
    """check that roundtrip of camera model to/from a bagfile works"""
    cam = _build_test_camera(**cam_opts)
    fname = tempfile.mktemp(suffix='.bag')
    try:
        with open(fname,mode='wb') as fd:
            cam.save_to_bagfile(fd, roslib)

        with open(fname,mode='r') as fd:
            bag = rosbag.Bag(fd, 'r')
            cam2 = CameraModel.load_camera_from_opened_bagfile( bag )
    finally:
        os.unlink(fname)

    verts = np.array([[ 0.042306,  0.015338,  0.036328],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.03323,   0.030344,  0.041542],
                      [ 0.036396,  0.026464,  0.052408]])

    expected =  cam.project_3d_to_pixel(verts)
    actual   = cam2.project_3d_to_pixel(verts)
    assert np.allclose( expected, actual )

 def from_dict(cls, d):
     cam_dict_list = d['camera_system']
     cams = [CameraModel.from_dict(cd) for cd in cam_dict_list]
     return MultiCameraSystem( cameras=cams )