'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.optimize import least_squares
from scipy.sparse import lil_matrix
from imu_lidar.geometry import so3_exp, so3_log
from imu_lidar.imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
from .rtk_imu_engineering import (
    G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)

NODE_DOF = 15
SIGMA_BG_RW = 1e-5
SIGMA_BA_RW = 1e-3

@dataclass(frozen=True)
class NodeGraphProblem:
    segment: _Segment
    R_seed_WI: tuple[np.ndarray,...]
    fixed_l_I_m: np.ndarray
    R_RTK_IMU: np.ndarray
    hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD

@dataclass(frozen=True)
class NodeGraphResult:
    success: bool
    message: str
    node_count: int
    duration_s: float
    fixed_l_I_m: np.ndarray
    initial_cost: float
    final_cost: float
    cost_reduction: float
    nfev: int
    optimality: float
    gradient_norm: float
    residual_dimension: int
    state_dimension: int
    statistical_dof: int
    total_nis: float
    chi_square_per_dof: float
    cost_per_dof: float
    initial_residual_by_factor: dict[str,dict[str,float]]
    final_residual_by_factor: dict[str,dict[str,float]]
    final_position_residual_m: dict[str,object]
    final_velocity_residual_m_s: dict[str,object]
    max_bg_step_rad_s: float
    max_ba_step_m_s2: float
    preintegration_covariance_sigma: dict[str,dict[str,float]]


@dataclass(frozen=True)
class FreeLeverResult:
    success: bool
    message: str
    initial_l_I_m: np.ndarray
    final_l_I_m: np.ndarray
    lever_step_norm_m: float
    initial_cost: float
    final_cost: float
    nfev: int
    optimality: float
    chi_square_per_dof: float
    position_residual_m: dict[str,object]
    velocity_residual_m_s: dict[str,object]
    residual_by_factor: dict[str,dict[str,float]]
    lever_covariance_m2: np.ndarray
    lever_information_singular_values: np.ndarray
    lever_information_condition_number: float
    lever_precision_rank: int
    weakest_lever_direction_I: np.ndarray

def _rotation(problem,index,x):
    offset = NODE_DOF*index
    return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])

def initial_parameters(problem, lever_override=None):
    nodes = problem.segment.nodes
    x = np.zeros(NODE_DOF*len(nodes))
    lever = (problem.fixed_l_I_m if lever_override is None
             else np.asarray(lever_override,dtype=float))
    previous_p, previous_v = np.zeros(3), np.zeros(3)
    for index,node in enumerate(nodes):
        offset = NODE_DOF*index
        R = problem.R_seed_WI[index]
        previous_p = node.p_enu_m-R@lever
        if index and not node.position_mask[2]:
            previous_p[2] = x[offset-NODE_DOF+5]
        if node.velocity_enu_m_s is not None:
            previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
        x[offset+3:offset+6] = previous_p
        x[offset+6:offset+9] = previous_v
    return x

def _stats(values, effective_dof=None):
    a = np.asarray(values,dtype=float).reshape(-1)
    if not a.size:
        return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
                'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
                'chi_square_per_dof':np.nan}
    nis = float(np.dot(a,a))
    dof = int(a.size if effective_dof is None else effective_dof)
    return {'count':int(a.size),'dof':dof,
            'rms':float(np.sqrt(np.mean(a*a))),
            'p50_abs':float(np.percentile(np.abs(a),50.)),
            'p95_abs':float(np.percentile(np.abs(a),95.)),
            'p99_abs':float(np.percentile(np.abs(a),99.)),
            'nis':nis,'chi_square_per_dof':nis/max(dof,1)}

def _hpr_sigma(problem, node):
    old = float(node.hpr_angular_sigma_rad)
    extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
    return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))


def _distribution(values):
    a = np.asarray(values,dtype=float).reshape(-1)
    if not a.size:
        return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
    return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
            'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
            'p99':float(np.percentile(a,99.))}

def _vector_stats(values):
    a = np.asarray(values,dtype=float).reshape(-1,3)
    if not a.size:
        return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
                'vector_rms':np.nan,'vector_p95':np.nan}
    norm = np.linalg.norm(a,axis=1)
    return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
            'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
            'vector_rms':float(np.sqrt(np.mean(norm*norm))),
            'vector_p95':float(np.percentile(norm,95.))}

def residual(problem,x,details=None,dependencies=None,lever_override=None):
    nodes, preints = problem.segment.nodes, problem.segment.preintegrations
    lever = (problem.fixed_l_I_m if lever_override is None
             else np.asarray(lever_override,dtype=float))
    baseline_I = problem.R_RTK_IMU.T[:,0]
    values = []
    def add(value,label,node_indices,raw=None):
        a = np.asarray(value,dtype=float).reshape(-1)
        values.extend(a)
        if dependencies is not None:
            dependencies.extend([tuple(node_indices)]*len(a))
        if details is not None:
            details.setdefault(label,[]).extend(a.tolist())
            if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
    for index,node in enumerate(nodes):
        offset = NODE_DOF*index
        R = _rotation(problem,index,x)
        p, v = x[offset+3:offset+6], x[offset+6:offset+9]
        bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
        p_error = p+R@lever-node.p_enu_m
        if node.source == 'GGA':
            add(p_error[:2]/.06,'gga_xy',(index,))
            if details is not None: details.setdefault('position_physical',[]).append(
                np.array([p_error[0],p_error[1],np.nan]))
        else:
            add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
            if details is not None: details.setdefault('position_physical',[]).append(p_error)
        if node.velocity_enu_m_s is not None:
            v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
            add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
            if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
        if node.hpr_factor_valid:
            hpr_error=np.cross(R@baseline_I,node.baseline_enu)
            add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
        if node.gravity_candidate:
            gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
            add(gravity/.12,'gravity',(index,))
        if index == len(nodes)-1: continue
        right = index+1
        right_offset = NODE_DOF*right
        Rj = _rotation(problem,right,x)
        pj = x[right_offset+3:right_offset+6]
        vj = x[right_offset+6:right_offset+9]
        bgj = x[right_offset+9:right_offset+12]
        baj = x[right_offset+12:right_offset+15]
        pre = preints[index]
        dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
        dt = pre.duration_s
        imu_error = np.concatenate([
            so3_log(dR.T@R.T@Rj),
            R.T@(vj-v-G_ENU*dt)-dv,
            R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
        add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
            (index,right),imu_error)
        add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
        add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
    add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
    add(x[9:12]/.02,'initial_gyro_bias',(0,))
    add(x[12:15]/.5,'initial_accel_bias',(0,))
    return np.asarray(values)

def jacobian_sparsity(problem,x):
    dependencies = []
    base = residual(problem,x,dependencies=dependencies)
    sparsity = lil_matrix((len(base),len(x)),dtype=int)
    for row,node_indices in enumerate(dependencies):
        for index in node_indices:
            start = NODE_DOF*index
            sparsity[row,start:start+NODE_DOF] = 1
    return sparsity.tocsr()

def _factor_stats(details):
    return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
            for key,value in details.items()
            if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}


def _effective_residual_dimension(details):
    return sum(2*len(v)//3 if k=='hpr' else len(v)
               for k,v in details.items() if not k.endswith('_physical')
               and k not in ('position_physical','velocity_physical'))

def _preintegration_covariance_stats(problem):
    blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
    for pre in problem.segment.preintegrations:
        sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
        blocks['rotation_rad'].extend(sigma[:3])
        blocks['velocity_m_s'].extend(sigma[3:6])
        blocks['position_m'].extend(sigma[6:9])
    return {key:_distribution(value) for key,value in blocks.items()}

def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
                  hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
    seeds = []
    for index,node in enumerate(segment.nodes):
        if node.hpr_factor_valid:
            seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
        elif index:
            seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
        else:
            seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
    return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
                            np.asarray(R_RTK_IMU,dtype=float),
                            float(hpr_direct_angular_sigma_rad))

def solve_fixed_lever(problem,max_nfev=30):
    x0 = initial_parameters(problem)
    initial_detail = {}
    r0 = residual(problem,x0,initial_detail)
    fit = least_squares(
        lambda value:residual(problem,value),x0,jac='2-point',
        jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
        tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
    final_detail = {}
    rf = residual(problem,fit.x,final_detail)
    statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
    bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
    ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
    bg_step = np.diff(bg,axis=0)
    ba_step = np.diff(ba,axis=0)
    return NodeGraphResult(
        success=bool(fit.success),message=str(fit.message),
        node_count=len(problem.segment.nodes),
        duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
        fixed_l_I_m=problem.fixed_l_I_m.copy(),
        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
        cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
        nfev=int(fit.nfev),optimality=float(fit.optimality),
        gradient_norm=float(np.linalg.norm(fit.grad)),
        residual_dimension=len(rf),state_dimension=len(fit.x),
        statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
        chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
        cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
        initial_residual_by_factor=_factor_stats(initial_detail),
        final_residual_by_factor=_factor_stats(final_detail),
        final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
        final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
        max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
        max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
        preintegration_covariance_sigma=_preintegration_covariance_stats(problem))


def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
    lever=np.asarray(l_I_m,dtype=float)
    x0=(initial_parameters(problem,lever) if initial_state_values is None
        else np.asarray(initial_state_values,dtype=float))
    r0=residual(problem,x0,lever_override=lever)
    fit=least_squares(
        lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
        jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
        loss='linear',max_nfev=max_nfev,x_scale='jac',
        ftol=1e-6,xtol=1e-6,gtol=1e-6)
    return fit.x,{'success':bool(fit.success),'message':str(fit.message),
                  'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
                  'cost':float(fit.cost)}


def summarize_fixed_state_values(problems,state_values,l_I_m):
    details={}; residuals=[]
    for problem,value in zip(problems,state_values):
        local={}
        residuals.append(residual(problem,np.asarray(value),details=local,
                                  lever_override=l_I_m))
        for key,items in local.items(): details.setdefault(key,[]).extend(items)
    joined=np.concatenate(residuals)
    state_dimension=sum(len(value) for value in state_values)
    dof=max(_effective_residual_dimension(details)-state_dimension,1)
    return {'cost':.5*float(np.dot(joined,joined)),
        'total_nis':float(np.dot(joined,joined)),
        'chi_square_per_dof':float(np.dot(joined,joined)/dof),
        'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
        'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
        'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
        'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}

def solve_fixed_lever_many(problems,max_nfev=30):
    problems = tuple(problems)
    sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
    offsets = np.cumsum([0,*sizes])
    x0 = np.concatenate([initial_parameters(problem) for problem in problems])
    def evaluate(value,details=None):
        chunks = []
        for index,problem in enumerate(problems):
            local_details = {} if details is not None else None
            chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
                                   local_details))
            if details is not None:
                for key,items in local_details.items():
                    details.setdefault(key,[]).extend(items)
        return np.concatenate(chunks)
    initial_detail = {}
    r0 = evaluate(x0,initial_detail)
    sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
    row = 0
    for index,problem in enumerate(problems):
        local_x = x0[offsets[index]:offsets[index+1]]
        local = jacobian_sparsity(problem,local_x)
        sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
        row += local.shape[0]
    fit = least_squares(
        lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
        method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
    final_detail = {}
    rf = evaluate(fit.x,final_detail)
    bg_steps, ba_steps = [], []
    for index,problem in enumerate(problems):
        states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
        bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
        ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
    covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
    for problem in problems:
        for pre in problem.segment.preintegrations:
            sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
            covariance['rotation_rad'].extend(sigma[:3])
            covariance['velocity_m_s'].extend(sigma[3:6])
            covariance['position_m'].extend(sigma[6:9])
    dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
    return NodeGraphResult(
        success=bool(fit.success),message=str(fit.message),
        node_count=sum(len(problem.segment.nodes) for problem in problems),
        duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
                       for problem in problems),
        fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
        cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
        nfev=int(fit.nfev),optimality=float(fit.optimality),
        gradient_norm=float(np.linalg.norm(fit.grad)),
        residual_dimension=len(rf),state_dimension=len(fit.x),
        statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
        chi_square_per_dof=float(np.dot(rf,rf)/dof),
        cost_per_dof=.5*float(np.dot(rf,rf)/dof),
        initial_residual_by_factor=_factor_stats(initial_detail),
        final_residual_by_factor=_factor_stats(final_detail),
        final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
        final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
        max_bg_step_rad_s=float(max(bg_steps,default=0.)),
        max_ba_step_m_s2=float(max(ba_steps,default=0.)),
        preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})


def _free_residual(problem,value,details=None):
    return residual(problem,value[3:],details=details,lever_override=value[:3])


def _free_sparsity(problem,value):
    local = jacobian_sparsity(problem,value[3:])
    result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
    result[:,:3] = 1
    result[:,3:] = local
    return result.tocsr()


def _marginal_lever_information(jacobian):
    J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
    H = J.T@J
    Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
    marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
    return .5*(marginal+marginal.T)


def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
    total=np.zeros((3,3))
    for index in range(len(row_offsets)-1):
        rows=slice(row_offsets[index],row_offsets[index+1])
        columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
        local=jacobian[rows,:][:,columns]
        total+=_marginal_lever_information(local)
    return .5*(total+total.T)


def linearized_lever_information(problem,l_I_m):
    lever=np.asarray(l_I_m,dtype=float)
    value=np.concatenate([lever,initial_parameters(problem,lever)])
    fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
        jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
        loss='linear',max_nfev=1,x_scale='jac')
    information=_marginal_lever_information(fit.jac)
    _,singular,Vt=np.linalg.svd(information)
    covariance=np.linalg.pinv(information,rcond=1e-9)
    return information,covariance,singular,Vt[-1]


def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
    initial_l = np.asarray(initial_l_I_m,dtype=float)
    x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
    r0 = _free_residual(problem,x0)
    fit = least_squares(
        lambda value:_free_residual(problem,value),x0,jac='2-point',
        jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
        loss='linear',max_nfev=max_nfev,x_scale='jac',
        ftol=1e-6,xtol=1e-6,gtol=1e-6)
    detail = {}
    rf = _free_residual(problem,fit.x,detail)
    information = _marginal_lever_information(fit.jac)
    _,singular_values,Vt = np.linalg.svd(information)
    tolerance = max(singular_values[0]*1e-9,1e-10)
    rank = int(np.sum(singular_values>tolerance))
    covariance = np.linalg.pinv(information,rcond=1e-9)
    dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
    condition = (float(singular_values[0]/singular_values[-1])
                 if singular_values[-1]>tolerance else np.inf)
    return FreeLeverResult(
        success=bool(fit.success),message=str(fit.message),
        initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
        lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
        initial_cost=.5*float(np.dot(r0,r0)),
        final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
        optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
        position_residual_m=_vector_stats(detail.get('position_physical',[])),
        velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
        residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
        lever_information_singular_values=singular_values,
        lever_information_condition_number=condition,lever_precision_rank=rank,
        weakest_lever_direction_I=Vt[-1].copy())


def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
                          initial_state_values=None,lever_prior_mean_m=None,
                          lever_prior_covariance_m2=None,return_state_values=False):
    problems=tuple(problems)
    initial_l=np.asarray(initial_l_I_m,dtype=float)
    sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
    offsets=np.cumsum([3,*sizes])
    states=([initial_parameters(problem,initial_l) for problem in problems]
            if initial_state_values is None else
            [np.asarray(value,dtype=float) for value in initial_state_values])
    prior_mean=(None if lever_prior_mean_m is None else
                np.asarray(lever_prior_mean_m,dtype=float))
    prior_cov=(None if lever_prior_covariance_m2 is None else
               np.asarray(lever_prior_covariance_m2,dtype=float))
    prior_whitener=(None if prior_cov is None else
                    np.linalg.inv(np.linalg.cholesky(prior_cov)))
    x0=np.concatenate([initial_l,*states])
    def evaluate(value,details=None):
        chunks=[]
        for index,problem in enumerate(problems):
            local={} if details is not None else None
            chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
                                   details=local,lever_override=value[:3]))
            if details is not None:
                for key,items in local.items(): details.setdefault(key,[]).extend(items)
        if prior_whitener is not None:
            prior_error=prior_whitener@(value[:3]-prior_mean)
            chunks.append(prior_error)
            if details is not None:
                details.setdefault('lever_prior',[]).extend(prior_error.tolist())
        return np.concatenate(chunks)
    r0=evaluate(x0)
    sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
    row=0; row_offsets=[0]
    for index,problem in enumerate(problems):
        local=jacobian_sparsity(problem,states[index])
        sparsity[row:row+local.shape[0],:3]=1
        sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
        row+=local.shape[0]
        row_offsets.append(row)
    if prior_whitener is not None:
        sparsity[row:row+3,:3]=1
    fit=least_squares(
        lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
        method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
        x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
    detail={}
    rf=evaluate(fit.x,detail)
    information=_additive_marginal_lever_information(
        fit.jac,row_offsets,offsets)
    if prior_cov is not None:
        information+=np.linalg.inv(prior_cov)
    _,singular_values,Vt=np.linalg.svd(information)
    tolerance=max(singular_values[0]*1e-9,1e-10)
    rank=int(np.sum(singular_values>tolerance))
    covariance=np.linalg.pinv(information,rcond=1e-9)
    dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
    condition=(float(singular_values[0]/singular_values[-1])
               if singular_values[-1]>tolerance else np.inf)
    result=FreeLeverResult(
        success=bool(fit.success),message=str(fit.message),
        initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
        lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
        initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
        nfev=int(fit.nfev),optimality=float(fit.optimality),
        chi_square_per_dof=float(np.dot(rf,rf)/dof),
        position_residual_m=_vector_stats(detail.get('position_physical',[])),
        velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
        residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
        lever_information_singular_values=singular_values,
        lever_information_condition_number=condition,lever_precision_rank=rank,
        weakest_lever_direction_I=Vt[-1].copy())
    if return_state_values:
        states=[fit.x[offsets[i]:offsets[i+1]].copy()
                for i in range(len(problems))]
        return result,states
    return result
