"""
MikoshiLang Extended Functions - Set 23
Robotics, control systems, and mechatronics
"""
import sympy as sp
from sympy import symbols, sqrt, sin, cos, exp, log, pi, atan2

def get_rules():
    """Extended set 23: Robotics and control systems (150 functions)"""
    x, y, z, t, s = symbols('x y z t s')
    
    return [
        # ===== KINEMATICS (35 functions) =====
        ('ForwardKinematics[theta_, link_lengths_]', lambda theta, L: (sum(Li*cos(ti) for Li, ti in zip(L, theta)), sum(Li*sin(ti) for Li, ti in zip(L, theta)))),
        ('InverseKinematics[x_, y_, L1_, L2_]', lambda x, y, L1, L2: (atan2(y, x), atan2(sqrt(x**2 + y**2 - L1**2), L1))),
        ('JacobianMatrix[f_, q_]', lambda f, q: sp.Matrix([[sp.diff(fi, qi) for qi in q] for fi in f])),
        ('VelocityKinematics[J_, q_dot_]', lambda J, qdot: J*qdot),
        ('DHTransform[a_, alpha_, d_, theta_]', lambda a, alpha, d, theta: sp.Matrix([[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)],
                                                                                        [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)],
                                                                                        [0, sin(alpha), cos(alpha), d],
                                                                                        [0, 0, 0, 1]])),  # Alias
        ('DHParameters[a_, alpha_, d_, theta_]', lambda a, alpha, d, theta: sp.Matrix([[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)],
                                                                                        [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)],
                                                                                        [0, sin(alpha), cos(alpha), d],
                                                                                        [0, 0, 0, 1]])),
        ('TransformationMatrix[R_, p_]', lambda R, p: sp.Matrix([[R, p], [0, 1]])),  # Simplified
        ('RotationMatrixX[theta_]', lambda theta: sp.Matrix([[1, 0, 0], [0, cos(theta), -sin(theta)], [0, sin(theta), cos(theta)]])),
        ('RotationMatrixY[theta_]', lambda theta: sp.Matrix([[cos(theta), 0, sin(theta)], [0, 1, 0], [-sin(theta), 0, cos(theta)]])),
        ('RotationMatrixZ[theta_]', lambda theta: sp.Matrix([[cos(theta), -sin(theta), 0], [sin(theta), cos(theta), 0], [0, 0, 1]])),
        ('EulerAngles[R_]', lambda R: (atan2(R[2,1], R[2,2]), atan2(-R[2,0], sqrt(R[2,1]**2 + R[2,2]**2)), atan2(R[1,0], R[0,0]))),
        ('QuaternionFromEuler[roll_, pitch_, yaw_]', lambda r, p, y: (sp.Symbol('q0'), sp.Symbol('q1'), sp.Symbol('q2'), sp.Symbol('q3'))),
        ('QuaternionMultiply[q1_, q2_]', lambda q1, q2: (q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3],
                                                          q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2],
                                                          q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1],
                                                          q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0])),
        ('QuaternionConjugate[q_]', lambda q: (q[0], -q[1], -q[2], -q[3])),
        ('QuaternionNorm[q_]', lambda q: sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2)),
        ('AxisAngleToRotation[axis_, angle_]', lambda ax, ang: sp.Matrix.eye(3)),  # Placeholder
        ('SLERP[q1_, q2_, t_]', lambda q1, q2, t: [(1-t)*q1i + t*q2i for q1i, q2i in zip(q1, q2)]),  # Simplified
        ('VelocityTransformation[omega_, v_, T_]', lambda omega, v, T: (omega, v)),
        ('ScewSymmetric[omega_]', lambda omega: sp.Matrix([[0, -omega[2], omega[1]], [omega[2], 0, -omega[0]], [-omega[1], omega[0], 0]])),
        ('TwistVector[v_, omega_]', lambda v, omega: sp.Matrix([v, omega])),
        ('WrenchVector[f_, tau_]', lambda f, tau: sp.Matrix([f, tau])),
        ('ManipulabilityIndex[J_]', lambda J: sqrt(sp.det(J*J.T))),
        ('ConditionNumber[J_]', lambda J: sp.Symbol('sigma_max')/sp.Symbol('sigma_min')),
        ('Singularity[det_J_]', lambda detJ: detJ == 0),
        ('Workspace[reachable_points_]', lambda pts: pts),
        ('DexterityMeasure[J_]', lambda J: 1/sp.Symbol('condition_number')),
        ('IsotropyIndex[J_]', lambda J: sp.Symbol('sigma_min')/sp.Symbol('sigma_max')),
        ('JointLimits[q_, q_min_, q_max_]', lambda q, qmin, qmax: qmin <= q <= qmax),
        ('SelfCollisionDetection[link_positions_]', lambda pos: False),
        ('PathPlanning[start_, goal_, obstacles_]', lambda s, g, obs: [s, g]),
        ('RRTAlgorithm[start_, goal_]', lambda s, g: [s, g]),
        ('PRMAlgorithm[nodes_, edges_]', lambda n, e: (n, e)),
        ('AStarPathPlanning[start_, goal_, heuristic_]', lambda s, g, h: [s, g]),
        ('DijkstraPathPlanning[start_, goal_]', lambda s, g: [s, g]),
        ('PotentialFieldMethod[q_, q_goal_, obstacles_]', lambda q, qg, obs: qg - q),
        ('TrajectoryGeneration[waypoints_, time_]', lambda wp, t: wp),
        
        # ===== DYNAMICS (30 functions) =====
        ('LagrangianMechanics[T_, V_]', lambda T, V: T - V),
        ('EulerLagrangeEquation[L_, q_]', lambda L, q: sp.diff(sp.diff(L, sp.diff(q, sp.Symbol('t'))), sp.Symbol('t')) - sp.diff(L, q)),
        ('KineticEnergy Robot[m_, v_, I_, omega_]', lambda m, v, I, omega: sp.Rational(1,2)*m*v**2 + sp.Rational(1,2)*I*omega**2),
        ('PotentialEnergyRobot[m_, g_, h_]', lambda m, g, h: m*g*h),
        ('NewtonEulerEquation[m_, a_, F_]', lambda m, a, F: m*a - F),
        ('InertiaMatrix[M_]', lambda M: M),
        ('CoriolisMatrix[C_]', lambda C: C),
        ('GravityVector[g_]', lambda g: g),
        ('JointTorque[tau_]', lambda tau: tau),
        ('InverseDynamics[M_, q_ddot_, C_, q_dot_, g_]', lambda M, qddot, C, qdot, g: M*qddot + C*qdot + g),
        ('ForwardDynamics[M_, tau_, C_, q_dot_, g_]', lambda M, tau, C, qdot, g: M.inv()*(tau - C*qdot - g)),
        ('RecursiveNewtonEuler[links_]', lambda links: links),
        ('ArticulatedBodyAlgorithm[tree_]', lambda tree: tree),
        ('CompositeRigidBodyAlgorithm[M_]', lambda M: M),
        ('CentrifugalForce[m_, omega_, r_]', lambda m, omega, r: m*omega**2*r),
        ('CoriolisForceRobot[m_, v_, omega_]', lambda m, v, omega: 2*m*v*omega),
        ('FrictionTorque[b_, q_dot_]', lambda b, qdot: b*qdot),
        ('DampingCoefficient[b_]', lambda b: b),
        ('SpringTorque[k_, q_]', lambda k, q: k*q),
        ('ActuatorDynamics[tau_motor_, gear_ratio_]', lambda tm, gr: tm*gr),
        ('MotorTorqueConstant[Kt_]', lambda Kt: Kt),
        ('BackEMFConstant[Ke_]', lambda Ke: Ke),
        ('MotorVoltageEquation[V_, R_, I_, Ke_, omega_]', lambda V, R, I, Ke, omega: V - R*I - Ke*omega),
        ('MotorCurrentEquation[tau_, Kt_]', lambda tau, Kt: tau/Kt),
        ('GearRatioRobot[N_in_, N_out_]', lambda Nin, Nout: Nin/Nout),
        ('EfficiencyTransmission[eta_]', lambda eta: eta),
        ('LoadInertiaReflected[I_load_, gear_ratio_]', lambda Iload, gr: Iload/gr**2),
        ('EquivalentInertia[I_motor_, I_load_reflected_]', lambda Im, Ilr: Im + Ilr),
        ('ZeromomentPoint[F_, M_]', lambda F, M: M/F),
        ('DynamicBalance[ZMP_, support_polygon_]', lambda ZMP, poly: ZMP in poly),
        
        # ===== CONTROL SYSTEMS (50 functions) =====
        ('PIDControl[Kp_, Ki_, Kd_, e_, int_e_, de_dt_]', lambda Kp, Ki, Kd, e, inte, dedt: Kp*e + Ki*inte + Kd*dedt),
        ('ProportionalControl[Kp_, e_]', lambda Kp, e: Kp*e),
        ('IntegralControl[Ki_, int_e_]', lambda Ki, inte: Ki*inte),
        ('DerivativeControl[Kd_, de_dt_]', lambda Kd, dedt: Kd*dedt),
        ('ErrorSignal[r_, y_]', lambda r, y: r - y),
        ('TransferFunctionControl[num_, den_]', lambda num, den: num/den),
        ('LaplaceDomain[time_function_]', lambda f: sp.Symbol('F(s)')),
        ('ClosedLoopTransferFunction[G_, H_]', lambda G, H: G/(1 + G*H)),
        ('OpenLoopGain[G_, H_]', lambda G, H: G*H),
        ('StabilityMargin[phase_, gain_]', lambda PM, GM: (PM, GM)),
        ('PhaseMargin[system_]', lambda sys: 180 + sp.Symbol('phase_at_gain_crossover')),
        ('GainMargin[system_]', lambda sys: -sp.Symbol('gain_at_phase_crossover')),
        ('RouthCriterion[coeffs_]', lambda c: True),  # Simplified stability test
        ('NyquistCriterion[GH_]', lambda GH: True),
        ('BodePlot[G_, omega_]', lambda G, omega: (20*log(abs(G), 10), sp.arg(G)*180/pi)),
        ('StepResponse[G_]', lambda G: sp.Symbol('y(t)')),
        ('ImpulseResponse[G_]', lambda G: sp.Symbol('h(t)')),
        ('RampResponse[G_]', lambda G: sp.Symbol('r(t)')),
        ('RiseTime[response_]', lambda resp: sp.Symbol('t_r')),
        ('SettlingTime[response_, tolerance_]', lambda resp, tol: sp.Symbol('t_s')),
        ('Overshoot[peak_, steady_state_]', lambda peak, ss: (peak - ss)/ss*100),
        ('SteadyStateError[r_, y_inf_]', lambda r, yinf: r - yinf),
        ('DampingRatioControl[zeta_]', lambda zeta: zeta),
        ('NaturalFrequencyControl[omega_n_]', lambda omegan: omegan),
        ('PolesControl[characteristic_eq_]', lambda char: sp.solve(char, sp.Symbol('s'))),
        ('ZerosControl[numerator_]', lambda num: sp.solve(num, sp.Symbol('s'))),
        ('RootLocusPlot[K_, poles_]', lambda K, poles: poles),
        ('StateSpaceModel[A_, B_, C_, D_]', lambda A, B, C, D: (A, B, C, D)),
        ('StateEquation[A_, x_, B_, u_]', lambda A, x, B, u: A*x + B*u),
        ('OutputEquation[C_, x_, D_, u_]', lambda C, x, D, u: C*x + D*u),
        ('Controllability[A_, B_]', lambda A, B: sp.Matrix.hstack(*[B, A*B, A**2*B])),
        ('Observability[A_, C_]', lambda A, C: sp.Matrix.vstack(*[C, C*A, C*A**2])),
        ('StateF eedback[K_, x_]', lambda K, x: -K*x),
        ('ObserverDesign[A_, L_, C_]', lambda A, L, C: A - L*C),
        ('KalmanFilter[A_, B_, C_, Q_, R_]', lambda A, B, C, Q, R: sp.Symbol('x_hat')),
        ('LQRControl[A_, B_, Q_, R_]', lambda A, B, Q, R: sp.Symbol('K')),
        ('LQGControl[A_, B_, C_, Q_, R_]', lambda A, B, C, Q, R: sp.Symbol('K')),
        ('H InfinityControl[plant_, weights_]', lambda plant, w: sp.Symbol('K')),
        ('RobustStability[uncertainty_, controller_]', lambda unc, K: True),
        ('AdaptiveControl[plant_, reference_model_]', lambda plant, ref: sp.Symbol('theta_hat')),
        ('ModelReferenceAdaptive[e_, gamma_]', lambda e, gamma: gamma*e),
        ('SelfTuningRegulator[params_]', lambda params: params),
        ('SliddingModeControl[s_, eta_]', lambda s, eta: -eta*sp.sign(s)),
        ('BacksteppingControl[error_dynamics_]', lambda edyn: sp.Symbol('u')),
        ('FeedbackLinearization[f_, g_, u_]', lambda f, g, u: (sp.Symbol('v') - f)/g),
        ('OptimalControl[J_, constraints_]', lambda J, cons: sp.Symbol('u_opt')),
        ('PontryaginMinimumPrinciple[H_]', lambda H: sp.diff(H, sp.Symbol('u'))),
        ('BellmanEquation[V_]', lambda V: sp.Symbol('V*')),
        ('ModelPredictiveControl[model_, horizon_, constraints_]', lambda model, N, cons: sp.Symbol('u_mpc')),
        ('RecedingHorizon[N_]', lambda N: N),
        
        # ===== SENSORS & PERCEPTION (20 functions) =====
        ('EncoderResolution[pulses_per_rev_]', lambda ppr: 360/ppr),
        ('PositionFromEncoder[counts_, resolution_]', lambda cnt, res: cnt*res),
        ('VelocityFromEncoder[delta_counts_, delta_t_, resolution_]', lambda dc, dt, res: dc*res/dt),
        ('AccelerometerOutput[a_]', lambda a: a),
        ('GyroscopeOutput[omega_]', lambda omega: omega),
        ('MagnetometerOutput[B_]', lambda B: B),
        ('IMUFusion[accel_, gyro_, mag_]', lambda a, g, m: (a, g, m)),
        ('ComplementaryFilter[accel_angle_, gyro_rate_, alpha_, dt_]', lambda aa, gr, alpha, dt: alpha*(aa + gr*dt) + (1 - alpha)*aa),
        ('KalmanFilterIMU[x_, P_, z_, Q_, R_]', lambda x, P, z, Q, R: x),
        ('MadgwickFilter[q_, gyro_, accel_, mag_]', lambda q, g, a, m: q),
        ('LiDARMeasurement[distance_, angle_]', lambda d, theta: (d*cos(theta), d*sin(theta))),
        ('PointCloudProcessing[points_]', lambda pts: pts),
        ('RANSAC[data_, model_]', lambda data, model: model),
        ('ICPAlgorithm[source_, target_]', lambda src, tgt: sp.Symbol('T')),
        ('VisualOdometry[images_]', lambda imgs: sp.Symbol('pose')),
        ('SLAM[sensors_, map_]', lambda sens, map: (sp.Symbol('pose'), map)),
        ('ParticleFilter[particles_, weights_, measurement_]', lambda p, w, z: p),
        ('OccupancyGrid[sensor_data_]', lambda data: sp.Matrix([[0]])),
        ('VoxelGrid[point_cloud_, resolution_]', lambda pc, res: pc),
        ('FeatureDetection[image_]', lambda img: [(x, y)]),
        ('FeatureMatching[features1_, features2_]', lambda f1, f2: list(zip(f1, f2))),
        
        # ===== ACTUATION (15 functions) =====
        ('ServoControl[PWM_]', lambda PWM: PWM),
        ('StepperMotorSteps[angle_, step_angle_]', lambda ang, stepang: ang/stepang),
        ('DCMotorSpeed[V_, Ke_]', lambda V, Ke: V/Ke),
        ('BrushlessMotorControl[phase_currents_]', lambda i: i),
        ('PWMDutyCycle[on_time_, period_]', lambda ton, T: ton/T*100),
        ('HBridgeControl[direction_, PWM_]', lambda dir, pwm: (dir, pwm)),
        ('CurrentControl[i_desired_, i_actual_, R_]', lambda id, ia, R: (id - ia)*R),
        ('VelocityControl[omega_desired_, omega_actual_, Kp_]', lambda od, oa, Kp: Kp*(od - oa)),
        ('PositionControl[theta_desired_, theta_actual_, Kp_]', lambda td, ta, Kp: Kp*(td - ta)),
        ('TorqueControl[tau_desired_, Kt_]', lambda td, Kt: td/Kt),
        ('PneumaticActuator[pressure_, area_]', lambda P, A: P*A),
        ('HydraulicActuator[pressure_, area_]', lambda P, A: P*A),
        ('LinearActuator[force_, displacement_]', lambda F, d: F*d),
        ('GripperForce[actuator_force_, mechanical_advantage_]', lambda F, MA: F*MA),
        ('ComplianceControl[F_desired_, F_actual_, K_]', lambda Fd, Fa, K: (Fd - Fa)/K),
    ]


def register():
    """Register all extended23 rules"""
    from .extended_helper import convert_rules
    return convert_rules(get_rules())
