"""
逆运动学模块 - 纯Python实现，无外部依赖
"""

class InverseKinematics:
    """逆运动学求解器（纯Python实现）"""
    
    def __init__(self, forward_kinematics_func=None, jacobian_func=None):
        self.fk = forward_kinematics_func
        self.jac = jacobian_func
        
    def velocity_ik(self, q, v_desired, dt=0.01):
        """速度级逆运动学（雅可比伪逆法）"""
        import numpy as np
        
        if self.jac is None:
            # 如果没有提供雅可比函数，用数值法计算
            J = self._numerical_jacobian(q)
        else:
            J = self.jac(q)
            
        # 计算伪逆
        J_T = J.T
        JJ_T = J @ J_T
        try:
            J_inv = J_T @ np.linalg.inv(JJ_T + 1e-6 * np.eye(JJ_T.shape[0]))
        except:
            J_inv = np.linalg.pinv(J)
            
        q_dot = J_inv @ v_desired
        return q_dot
    
    def position_ik_newton(self, q_init, x_desired, max_iter=100, tol=1e-6, alpha=0.5):
        """位置级逆运动学（牛顿-拉弗森法）"""
        import numpy as np
        
        q = np.array(q_init, dtype=float).copy()
        
        for i in range(max_iter):
            # 计算当前末端位置
            x_current = self.fk(q)
            
            # 误差
            error = np.array(x_desired) - np.array(x_current)
            if np.linalg.norm(error) < tol:
                break
                
            # 计算雅可比并求伪逆
            J = self._numerical_jacobian(q)
            J_T = J.T
            JJ_T = J @ J_T
            try:
                J_inv = J_T @ np.linalg.inv(JJ_T + 1e-6 * np.eye(JJ_T.shape[0]))
            except:
                J_inv = np.linalg.pinv(J)
                
            # 更新关节角度
            delta_q = alpha * J_inv @ error
            q = q + delta_q
            
        return q
    
    def _numerical_jacobian(self, q, eps=1e-6):
        """数值法计算雅可比矩阵"""
        import numpy as np
        
        n = len(q)
        x0 = self.fk(q)
        m = len(x0)
        J = np.zeros((m, n))
        
        for i in range(n):
            q_plus = q.copy()
            q_plus[i] += eps
            x_plus = self.fk(q_plus)
            J[:, i] = (np.array(x_plus) - np.array(x0)) / eps
            
        return J