import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.interpolate import CubicSpline
from mpl_toolkits.mplot3d import Axes3D

class MagneticFieldLineODE:
    """
    Convert a magnetic field (vector field) into an ODE represented by cubic splines
    along a field line. The ODE is dr/ds = unit tangent, where s is arclength.
    """
    def __init__(self, points, B_func=None):
        """
        points: array of shape (N, 3) – positions along a field line.
        B_func: optional, the original magnetic field function B(r) for validation.
        """
        self.points = np.asarray(points)
        # Remove duplicate consecutive points
        diff = np.linalg.norm(np.diff(self.points, axis=0), axis=1)
        mask = np.concatenate(([True], diff > 1e-12))
        self.points = self.points[mask]
        # Compute cumulative arclength as parameter
        diffs = np.diff(self.points, axis=0)
        seg_lengths = np.linalg.norm(diffs, axis=1)
        self.s = np.concatenate(([0], np.cumsum(seg_lengths)))  # arclength parameter
        # Fit cubic splines for x(s), y(s), z(s)
        self.spline_x = CubicSpline(self.s, self.points[:, 0], bc_type='natural')
        self.spline_y = CubicSpline(self.s, self.points[:, 1], bc_type='natural')
        self.spline_z = CubicSpline(self.s, self.points[:, 2], bc_type='natural')
        self.B_func = B_func  # optional original field for validation
        
    def position(self, s):
        """Return r(s) = (x,y,z) at arclength s."""
        return np.array([self.spline_x(s), self.spline_y(s), self.spline_z(s)])
    
    def tangent(self, s):
        """Return dr/ds (unit tangent) at arclength s."""
        dx = self.spline_x.derivative()(s)
        dy = self.spline_y.derivative()(s)
        dz = self.spline_z.derivative()(s)
        return np.array([dx, dy, dz])
    
    def curvature(self, s):
        """Compute curvature κ(s) = |d²r/ds²|."""
        d2x = self.spline_x.derivative(2)(s)
        d2y = self.spline_y.derivative(2)(s)
        d2z = self.spline_z.derivative(2)(s)
        return np.linalg.norm([d2x, d2y, d2z])
    
    def get_coefficients(self):
        """
        Return cubic spline coefficients for each segment.
        For segment i (between s[i] and s[i+1]), x(s) = a*(s-si)^3 + b*(s-si)^2 + c*(s-si) + d.
        Similarly for y(s) and z(s).
        """
        coeffs_x = self.spline_x.c  # shape (4, n-1)
        coeffs_y = self.spline_y.c
        coeffs_z = self.spline_z.c
        knots = self.spline_x.x
        segments = []
        for i in range(len(knots)-1):
            segments.append({
                's_start': knots[i],
                's_end': knots[i+1],
                'coeffs_x': coeffs_x[:, i],
                'coeffs_y': coeffs_y[:, i],
                'coeffs_z': coeffs_z[:, i]
            })
        return segments
    
    def plot_field_line(self, ax=None, num_points=500):
        """Plot the field line from the spline."""
        s_fine = np.linspace(self.s[0], self.s[-1], num_points)
        r_fine = self.position(s_fine)
        if ax is None:
            fig = plt.figure()
            ax = fig.add_subplot(111, projection='3d')
        ax.plot(r_fine[0], r_fine[1], r_fine[2], 'b-', linewidth=2)
        ax.scatter(self.points[:,0], self.points[:,1], self.points[:,2], c='r', s=20)
        ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z')
        ax.set_title('Magnetic Field Line (cubic spline)')
        return ax

# -------------------------------------------------------------------
# Example: Dipole magnetic field of a bar magnet (magnetic moment along z)
# -------------------------------------------------------------------
def dipole_field(r, m=np.array([0, 0, 1])):
    """
    Magnetic field B(r) for a dipole moment m at origin.
    B = (mu0/4pi) * (3(m·r̂)r̂ - m)/r^3  (in units where mu0/4pi = 1)
    """
    x, y, z = r
    r_vec = np.array([x, y, z])
    r_norm = np.linalg.norm(r_vec)
    if r_norm < 1e-8:
        return np.zeros(3)
    r_hat = r_vec / r_norm
    m_dot_r = np.dot(m, r_hat)
    B = (3 * m_dot_r * r_hat - m) / (r_norm**3)
    return B

def trace_field_line(B_func, start_point, s_max=5.0, step_size=0.05):
    """
    Trace a field line by integrating dr/ds = B/|B| (unit tangent).
    s is arclength.
    """
    def ode(s, r):
        B = B_func(r)
        B_norm = np.linalg.norm(B)
        if B_norm < 1e-8:
            return np.zeros(3)
        return B / B_norm  # unit tangent
    
    sol = solve_ivp(ode, [0, s_max], start_point, method='RK45',
                    t_eval=np.arange(0, s_max, step_size),
                    rtol=1e-6, atol=1e-9)
    return sol.y.T  # points as (N, 3)

# -------------------------------------------------------------------
# Main: convert dipole field to ODE via cubic spline
# -------------------------------------------------------------------
if __name__ == "__main__":
    # Trace a field line starting from (1, 0, 0) in dipole field
    start = np.array([1.0, 0.0, 0.0])
    points = trace_field_line(dipole_field, start, s_max=8.0, step_size=0.05)
    
    # Create ODE representation using cubic splines
    ode_repr = MagneticFieldLineODE(points, B_func=dipole_field)
    
    # Plot the field line
    ax = ode_repr.plot_field_line()
    plt.show()
    
    # Output coefficients for first few segments
    segs = ode_repr.get_coefficients()
    print("Cubic spline coefficients for first segment (s ∈ [%.3f, %.3f]):" % (segs[0]['s_start'], segs[0]['s_end']))
    print("  x(s) = %.6f*(s-si)^3 + %.6f*(s-si)^2 + %.6f*(s-si) + %.6f" % tuple(segs[0]['coeffs_x']))
    print("  y(s) = %.6f*(s-si)^3 + %.6f*(s-si)^2 + %.6f*(s-si) + %.6f" % tuple(segs[0]['coeffs_y']))
    print("  z(s) = %.6f*(s-si)^3 + %.6f*(s-si)^2 + %.6f*(s-si) + %.6f" % tuple(segs[0]['coeffs_z']))
    
    # Validation: compare spline tangent with original B direction at a point
    s_test = 1.0
    r_test = ode_repr.position(s_test)
    tangent_spline = ode_repr.tangent(s_test)
    B_orig = dipole_field(r_test)
    B_unit_orig = B_orig / np.linalg.norm(B_orig)
    print("\nValidation at s = %.2f, r = (%.3f, %.3f, %.3f):" % (s_test, *r_test))
    print("  Spline tangent:  ", tangent_spline)
    print("  Original B unit: ", B_unit_orig)
    print("  Dot product (should be near 1): %.6f" % np.dot(tangent_spline, B_unit_orig))
    
    # ODE representation: dr/ds = tangent(spline). This is the ODE.
    # To solve it forward, one would evaluate tangent(s) at current s.
    print("\nThe ODE for the field line in spline form is:")
    print("  dr/ds = (dx/ds, dy/ds, dz/ds) where x(s), y(s), z(s) are the cubic splines.")
    print("The cubic coefficients above define these polynomials segment-wise.")