# Path: lfd/tpsopt/registration.py
# def unit_boxify(x_na):    
#     ranges = x_na.ptp(axis=0)
#     dlarge = ranges.argmax()
#     unscaled_translation = - (x_na.min(axis=0) + x_na.max(axis=0))/2
#     scaling = 1./ranges[dlarge]
#     scaled_translation = unscaled_translation * scaling
#     return x_na*scaling + scaled_translation, (scaling, scaled_translation)    
# 
# def loglinspace(a,b,n):
#     "n numbers between a to b (inclusive) with constant ratio between consecutive numbers"
#     return np.exp(np.linspace(np.log(a),np.log(b),n))    
# 
# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


import h5py
import argparse
import sys
import numpy as np
import scipy.linalg
import scikits.cuda.linalg
from pycuda import gpuarray
from scikits.cuda.linalg import pinv as cu_pinv
from settings import GRIPPER_OPEN_CLOSE_THRESH
from tps import tps_kernel_matrix, tps_kernel_matrix2
from lfd.tpsopt.registration import unit_boxify, loglinspace
from lfd.rapprentice import clouds
from culinalg_exts import get_gpu_ptrs, dot_batch_nocheck, m_dot_batch
from settings import N_ITER_CHEAP, DEFAULT_LAMBDA, DS_SIZE, BEND_COEF_DIGITS, EXACT_LAMBDA, N_ITER_EXACT
    
    mult = 5.0
    GRIPPER_L_FINGER_OPEN_CLOSE_THRESH = mult * GRIPPER_OPEN_CLOSE_THRESH

    # indices BEFORE transition occurs
    opening_inds = np.flatnonzero((finger_traj[1:] >= GRIPPER_L_FINGER_OPEN_CLOSE_THRESH) & (finger_traj[:-1] < GRIPPER_L_FINGER_OPEN_CLOSE_THRESH))
    closing_inds = np.flatnonzero((finger_traj[1:] < GRIPPER_L_FINGER_OPEN_CLOSE_THRESH) & (finger_traj[:-1] >= GRIPPER_L_FINGER_OPEN_CLOSE_THRESH))
    
    return opening_inds, closing_inds

def gripper_joint2gripper_l_finger_joint_values(gripper_joint_vals):
    """
    Only the %s_gripper_l_finger_joint%lr can be controlled (this is the joint returned by robot.GetManipulator({"l":"leftarm", "r":"rightarm"}[lr]).GetGripperIndices())
    The rest of the gripper joints (like %s_gripper_joint%lr) are mimiced and cannot be controlled directly
    """
    mult = 5.0
    gripper_l_finger_joint_vals = mult * gripper_joint_vals
    return gripper_l_finger_joint_vals



def downsample_cloud(cloud_xyz):
    return clouds.downsample(cloud_xyz, DS_SIZE)

def main():
    scikits.cuda.linalg.init()
    args = parse_arguments()

    f = h5py.File(args.datafile, 'r+')
    

# Path: lfd/tpsopt/registration.py
# def unit_boxify(x_na):    
#     ranges = x_na.ptp(axis=0)
#     dlarge = ranges.argmax()
#     unscaled_translation = - (x_na.min(axis=0) + x_na.max(axis=0))/2
#     scaling = 1./ranges[dlarge]
#     scaled_translation = unscaled_translation * scaling
#     return x_na*scaling + scaled_translation, (scaling, scaled_translation)    
# 
# def loglinspace(a,b,n):
#     "n numbers between a to b (inclusive) with constant ratio between consecutive numbers"
#     return np.exp(np.linspace(np.log(a),np.log(b),n))    
# 
# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


# Path: lfd/tpsopt/precompute.py
import h5py
import argparse
import sys
import numpy as np
import scipy.linalg
import scikits.cuda.linalg
from pycuda import gpuarray
from scikits.cuda.linalg import pinv as cu_pinv
from settings import GRIPPER_OPEN_CLOSE_THRESH
from tps import tps_kernel_matrix, tps_kernel_matrix2
from lfd.tpsopt.registration import unit_boxify, loglinspace
from lfd.rapprentice import clouds
from culinalg_exts import get_gpu_ptrs, dot_batch_nocheck, m_dot_batch
from settings import N_ITER_CHEAP, DEFAULT_LAMBDA, DS_SIZE, BEND_COEF_DIGITS, EXACT_LAMBDA, N_ITER_EXACT

    
    mult = 5.0
    GRIPPER_L_FINGER_OPEN_CLOSE_THRESH = mult * GRIPPER_OPEN_CLOSE_THRESH

    # indices BEFORE transition occurs
    opening_inds = np.flatnonzero((finger_traj[1:] >= GRIPPER_L_FINGER_OPEN_CLOSE_THRESH) & (finger_traj[:-1] < GRIPPER_L_FINGER_OPEN_CLOSE_THRESH))
    closing_inds = np.flatnonzero((finger_traj[1:] < GRIPPER_L_FINGER_OPEN_CLOSE_THRESH) & (finger_traj[:-1] >= GRIPPER_L_FINGER_OPEN_CLOSE_THRESH))
    
    return opening_inds, closing_inds

def gripper_joint2gripper_l_finger_joint_values(gripper_joint_vals):
    """
    Only the %s_gripper_l_finger_joint%lr can be controlled (this is the joint returned by robot.GetManipulator({"l":"leftarm", "r":"rightarm"}[lr]).GetGripperIndices())
    The rest of the gripper joints (like %s_gripper_joint%lr) are mimiced and cannot be controlled directly
    """
    mult = 5.0
    gripper_l_finger_joint_vals = mult * gripper_joint_vals
    return gripper_l_finger_joint_vals



def downsample_cloud(cloud_xyz):
    return clouds.downsample(cloud_xyz, DS_SIZE)

def main():
    scikits.cuda.linalg.init()
    args = parse_arguments()

    f = h5py.File(args.datafile, 'r+')
    

    bend_coefs = np.around(loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter), 