import numpy as np
import algopy


def cost_function_euclidean_differences_algopy(params, matches):
    sina0, sina1, sina2 = algopy.sin(params[0]), algopy.sin(params[1]), algopy.sin(params[2]) 
    cosa0, cosa1, cosa2 = algopy.cos(params[0]), algopy.cos(params[1]), algopy.cos(params[2])  
    rotation = np.array([[cosa1 * cosa2, -cosa0 * sina2 + sina0 * sina1 * cosa2, sina0 * sina2 + cosa0 * sina1 * cosa2 ],
                        [cosa1 * sina2, cosa0 * cosa2 + sina0 * sina1 * sina2,  -sina1 * cosa2 + cosa0 * sina1 * sina2],
                        [-sina1,        sina0 * cosa1,                          cosa0 * cosa1                         ]])   
    translation = params[3:6]
    
    c_points = algopy.dot(rotation, matches['w_point'].T) + translation[:, np.newaxis]
    i_points = f * c_points[:2, :] / c_points[2, :]
    p_points = i_points + np.array([[cx], [cy]])  
    residual_differences = p_points -  matches['p_point'].T
    return residual_differences



matches = np.array([([900.0, 900.0, 2000.0], [478.5, 398.5]), 
                    ([500.0, 900.0, 2000.0], [408.5, 398.5]), 
                    ([100.0, 900.0, 2000.0], [337.5, 398.5]), 
                    ([-300.0, 900.0, 2000.0], [266.5, 398.5]), 
                    ([-600.0, 600.0, 2000.0], [213.5, 345.5])], 
                    dtype=[('w_point', '<f8', (3,)), ('p_point', '<f8', (2,))])

cx = 320
cy = 240
f = 708

params = np.array([0., 0., 0., 0., 0., 2000])


cg = algopy.CGraph()
params2 = algopy.Function(params)        
residuals = cost_function_euclidean_differences_algopy(params2, matches)
cg.trace_off()
cg.independentFunctionList = [params2]
cg.dependentFunctionList = [residuals]

print cg.jacobian(params)