def first_step_of_hw_working():
    f = 1
    K = np.identity(3)
    R = K.copy()
    C = np.array(np.arange(0, 3 * 1).reshape(3, 1), dtype=float)
    C[:, 0] = [1, 2, -3]

    X1d, X2d, X3d = get_first_hw_points()
    # X1d, X2d, X3d = get_second_hw_points()
    Pb = hw03.make_pb(K, R, C, f)
    X1b, X2b, X3b = project_3_points(Pb, X1d, X2d, X3d)

    X1c = from_Beta_to_Gama(K, X1b)
    X2c = from_Beta_to_Gama(K, X2b)
    X3c = from_Beta_to_Gama(K, X3b)

    c12, c23, c31 = compute_angles_cosines(X1c, X2c, X3c)
    d12, d23, d31 = compute_distances(X1d, X2d, X3d)

    N1, N2, N3 = p3p_distances(d12, d23, d31, c12, c23, c31)

    # N = [N1[0], N2[0], N3[0]]
    # u = np.array(np.arange(0, 2 * 3).reshape(2, 3), dtype=float)
    # X = np.array(np.arange(0, 3 * 3).reshape(3, 3), dtype=float)
    # u[:, 0] = np.reshape(X1a, (2,))
    # u[:, 1] = np.reshape(X2a, (2,))
    # u[:, 2] = np.reshape(X3a, (2,))
    #
    # X[:, 0] = np.reshape(X1d, (3,))
    # X[:, 1] = np.reshape(X2d, (3,))
    # X[:, 2] = np.reshape(X3d, (3,))
    #
    # R, C = p3p_RC(N, u, X, K)

    # Vectors from C to X in Eta base
    Y1e = normalize(X1c) * N1[0]
    Y2e = normalize(X2c) * N2[0]
    Y3e = normalize(X3c) * N3[0]

    Z2e = Y2e - Y1e
    Z3e = Y3e - Y1e
    Z1e = np.cross(Z2e.T, Z3e.T)

    Z2d = X2d - X1d
    Z3d = X3d - X1d
    Z1d = np.cross(Z2d.T, Z3d.T)

    ZE = np.array(np.arange(0, 3 * 3).reshape(3, 3), dtype=float)
    ZE[:, 0] = Z1e
    ZE[:, 1] = Z2e.T    # WTF why transpose
    ZE[:, 2] = Z3e.T

    ZD = np.array(np.arange(0, 3 * 3).reshape(3, 3), dtype=float)
    ZD[:, 0] = Z1d
    ZD[:, 1] = Z2d.T
    ZD[:, 2] = Z3d.T

    R = ZE @ invert(ZD)

    Cd1 = X1d - R.T @ Y1e
    Cd2 = X2d - R.T @ Y2e
    Cd3 = X3d - R.T @ Y3e