import numpy as np
from scipy.optimize import fsolve

# Crankshaft geometry
Lc = 7.0  # mm
La = 20.0  # mm


# Functions
def crank_position(theta : float):
    crank_contrib = Lc * np.cos(theta)
    arm_contrib = np.sqrt(La**2 - (Lc * np.sin(theta)) ** 2)
    return (Lc + La) - crank_contrib - arm_contrib


def crank_velocity(theta : float):
    sin_theta = np.sin(theta)
    cos_theta = np.cos(theta)
    denominator = np.sqrt(La**2 - (Lc * sin_theta) ** 2)
    return Lc * sin_theta - (Lc**2 * sin_theta * cos_theta) / denominator


def dneedle_dtheta(theta : float):
    # Derivative (Jacobian H)
    sin_t = np.sin(theta)
    cos_t = np.cos(theta)
    sqrt_term = np.sqrt(La**2 - (Lc * sin_t) ** 2)
    dcrank = -Lc * sin_t
    darm = (Lc**2 * sin_t * cos_t) / sqrt_term
    return -dcrank - darm



def find_theta(x, theta0=0.0):
    """
    Find theta for a given position x using numerical root-finding.

    Parameters:
        x (float): Desired position.
        Lc (float): Length of crank.
        La (float): Length of arm.
        theta0 (float): Initial guess for theta (default: 0.0).

    Returns:
        float: Theta that satisfies the equation.
    """
    def equation(theta):
        return crank_position(theta) - x

    theta_solution = fsolve(equation, theta0)[0]
    return theta_solution