import numpy as np
from scipy.io.wavfile import read
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# ============================================================
# Load MNIST Data
# ============================================================
X_train = read('../X_train.wav')[1].reshape(-1, 784).astype(np.float64) / 255.0
y_train = (read('../y_train.wav')[1] * 9).astype(int)
X_test = read('../X_test.wav')[1].reshape(-1, 784).astype(np.float64) / 255.0
y_test = (read('../y_test.wav')[1] * 9).astype(int)

print(f"Train: {X_train.shape}, Labels: {y_train.shape}")
print(f"Test: {X_test.shape}, Labels: {y_test.shape}")

# ============================================================
# Activation Functions
# ============================================================
def relu(x):
    return np.maximum(0, x)

def relu_derivative(x):
    return np.where(x > 0, 1.0, 0.0)

def sigmoid(x):
    return 1.0 / (1.0 + np.exp(-np.clip(x, -500, 500)))

def sigmoid_derivative(x):
    s = sigmoid(x)
    return s * (1 - s)

def softmax(x, axis=-1):
    exp_x = np.exp(x - np.max(x, axis=axis, keepdims=True))
    return exp_x / np.sum(exp_x, axis=axis, keepdims=True)

def tanh(x):
    return np.tanh(x)

def tanh_derivative(x):
    return 1 - np.tanh(x)**2


# ============================================================
# Quantum Vertex Generator (from scratch)
# ============================================================
class QuantumVertexGenerator:
    """
    Converts batch of images to 100 × 12 × 3 vertex points.
    Uses learned weights for feature extraction and vertex generation.
    """
    
    def __init__(self, input_size=784, num_vertices=12, batch_size=100, seed=42):
        np.random.seed(seed)
        
        self.num_vertices = num_vertices
        self.batch_size = batch_size
        self.input_size = input_size
        
        # Learnable base vertices
        self.base_vertices = np.random.randn(num_vertices, 3) * 0.5
        
        # Feature extraction network (Conv-like operations as matrix multiplications)
        # Layer 1: 784 -> 256
        self.W1 = np.random.randn(input_size, 256) * np.sqrt(2.0 / input_size)
        self.b1 = np.zeros(256)
        
        # Layer 2: 256 -> 128
        self.W2 = np.random.randn(256, 128) * np.sqrt(2.0 / 256)
        self.b2 = np.zeros(128)
        
        # Layer 3: 128 -> 64
        self.W3 = np.random.randn(128, 64) * np.sqrt(2.0 / 128)
        self.b3 = np.zeros(64)
        
        # Rotation predictor (quaternions for each vertex)
        self.W_rot = np.random.randn(64, num_vertices * 4) * 0.01
        self.b_rot = np.zeros(num_vertices * 4)
        
        # Scale predictor (for each vertex, 3 values)
        self.W_scale = np.random.randn(64, num_vertices * 3) * 0.01
        self.b_scale = np.zeros(num_vertices * 3)
        
        # Translation predictor (global, 3 values)
        self.W_trans = np.random.randn(64, 3) * 0.01
        self.b_trans = np.zeros(3)
        
        # Store activations for backprop
        self.cache = {}
    
    def forward(self, X):
        """
        Args:
            X: (batch, 784) normalized images
        Returns:
            vertices: (batch, num_vertices, 3) 3D vertex points
        """
        batch_size = X.shape[0]
        
        # Feature extraction
        self.cache['x0'] = X
        
        # Conv-like layer 1 (simulate 3x3 conv with stride by projection)
        h1 = X @ self.W1 + self.b1  # (batch, 256)
        h1 = relu(h1)
        self.cache['h1'] = h1
        
        # Layer 2
        h2 = h1 @ self.W2 + self.b2  # (batch, 128)
        h2 = relu(h2)
        self.cache['h2'] = h2
        
        # Layer 3
        h3 = h2 @ self.W3 + self.b3  # (batch, 64)
        h3 = relu(h3)
        self.cache['h3'] = h3
        
        # Predictions
        quaternions = h3 @ self.W_rot + self.b_rot  # (batch, num_vertices * 4)
        scales = sigmoid(h3 @ self.W_scale + self.b_scale)  # (batch, num_vertices * 3)
        translation = tanh(h3 @ self.W_trans + self.b_trans)  # (batch, 3)
        
        self.cache['quaternions'] = quaternions
        self.cache['scales'] = scales
        self.cache['translation'] = translation
        
        q = quaternions.reshape(batch_size, self.num_vertices, 4)
        q = q / (np.linalg.norm(q, axis=2, keepdims=True) + 1e-8)
        scales_reshaped = scales.reshape(batch_size, self.num_vertices, 3)
        
        rotated_vertices = self._rotate_vertices_by_quaternion(self.base_vertices, q)
        vertices = rotated_vertices * (scales_reshaped + 0.5) + translation[:, np.newaxis, :]
        
        self.cache['vertices'] = vertices
        return vertices
    
    def _rotate_by_quaternion(self, v, q):
        """Rotate vector v by quaternion q = [w, x, y, z]"""
        w, x, y, z = q[0], q[1], q[2], q[3]
        
        # Quaternion rotation matrix
        vx, vy, vz = v[0], v[1], v[2]
        
        result_x = 2*( (w*w + x*x - 0.5)*vx + (x*y - w*z)*vy + (x*z + w*y)*vz )
        result_y = 2*( (x*y + w*z)*vx + (w*w + y*y - 0.5)*vy + (y*z - w*x)*vz )
        result_z = 2*( (x*z - w*y)*vx + (y*z + w*x)*vy + (w*w + z*z - 0.5)*vz )
        
        return np.array([result_x, result_y, result_z])

    def _rotate_vertices_by_quaternion(self, base_vertices, quaternions):
        """Rotate all base vertices for a batch of quaternions."""
        w = quaternions[:, :, 0]
        x = quaternions[:, :, 1]
        y = quaternions[:, :, 2]
        z = quaternions[:, :, 3]

        vx = base_vertices[np.newaxis, :, 0]
        vy = base_vertices[np.newaxis, :, 1]
        vz = base_vertices[np.newaxis, :, 2]

        result_x = 2 * ((w*w + x*x - 0.5) * vx + (x*y - w*z) * vy + (x*z + w*y) * vz)
        result_y = 2 * ((x*y + w*z) * vx + (w*w + y*y - 0.5) * vy + (y*z - w*x) * vz)
        result_z = 2 * ((x*z - w*y) * vx + (y*z + w*x) * vy + (w*w + z*z - 0.5) * vz)

        return np.stack([result_x, result_y, result_z], axis=2)
    
    def backward(self, grad_vertices):
        """
        Backprop through vertex generation.
        grad_vertices: (batch, num_vertices, 3)
        """
        batch_size = grad_vertices.shape[0]
        
        # Gradients for transformations
        grad_quaternions = np.zeros((batch_size, self.num_vertices * 4))
        grad_scales = np.zeros((batch_size, self.num_vertices * 3))
        grad_translation = np.zeros((batch_size, 3))
        
        # Gradients for base vertices
        grad_base_vertices = np.zeros_like(self.base_vertices)
        
        for b in range(batch_size):
            for v in range(self.num_vertices):
                # Get stored values
                q = self.cache['quaternions'][b, v*4:(v+1)*4]
                scale_v = self.cache['scales'][b, v*3:(v+1)*3]
                
                q_norm = np.linalg.norm(q) + 1e-8
                q_normalized = q / q_norm
                
                w, x, y, z = q_normalized[0], q_normalized[1], q_normalized[2], q_normalized[3]
                v_base = self.base_vertices[v]
                
                # Gradient w.r.t. rotation (simplified)
                grad_q = np.zeros(4)
                gv = grad_vertices[b, v]
                
                # dR/dq approximation
                grad_q[0] = 2 * (x*gv[0] + y*gv[1] + z*gv[2])  # w component
                grad_q[1] = 2 * (w*gv[0] + z*gv[1] - y*gv[2])  # x component
                grad_q[2] = 2 * (-z*gv[0] + w*gv[1] + x*gv[2])  # y component
                grad_q[3] = 2 * (y*gv[0] - x*gv[1] + w*gv[2])  # z component
                
                grad_quaternions[b, v*4:(v+1)*4] = grad_q
                
                # Gradient w.r.t. scale
                grad_scales[b, v*3:(v+1)*3] = gv * v_base * sigmoid_derivative(scale_v)
                
                # Gradient w.r.t. translation
                grad_translation[b] += gv
                
                # Gradient w.r.t. base vertex
                grad_base_vertices[v] += gv * (scale_v + 0.5)
        
        # Backprop through transformations
        grad_h3_rot = grad_quaternions @ self.W_rot.T
        grad_h3_scale = grad_scales @ self.W_scale.T
        grad_h3_trans = grad_translation @ self.W_trans.T
        grad_h3 = grad_h3_rot + grad_h3_scale + grad_h3_trans
        
        # Backprop through layers
        grad_h2 = grad_h3 * relu_derivative(self.cache['h3'])
        grad_h2 = grad_h2 @ self.W3.T
        
        grad_h1 = grad_h2 * relu_derivative(self.cache['h2'])
        grad_h1 = grad_h1 @ self.W2.T
        
        grad_x0 = grad_h1 * relu_derivative(self.cache['h1'])
        grad_x0 = grad_x0 @ self.W1.T
        
        # Collect gradients for weights
        grad_weights = {
            'W1': self.cache['x0'].T @ (grad_h1 * relu_derivative(self.cache['h1'])),
            'b1': np.sum(grad_h1 * relu_derivative(self.cache['h1']), axis=0),
            'W2': self.cache['h1'].T @ (grad_h2 * relu_derivative(self.cache['h2'])),
            'b2': np.sum(grad_h2 * relu_derivative(self.cache['h2']), axis=0),
            'W3': self.cache['h2'].T @ (grad_h3 * relu_derivative(self.cache['h3'])),
            'b3': np.sum(grad_h3 * relu_derivative(self.cache['h3']), axis=0),
            'W_rot': self.cache['h3'].T @ grad_quaternions,
            'b_rot': np.sum(grad_quaternions, axis=0),
            'W_scale': self.cache['h3'].T @ grad_scales,
            'b_scale': np.sum(grad_scales, axis=0),
            'W_trans': self.cache['h3'].T @ grad_translation,
            'b_trans': np.sum(grad_translation, axis=0),
            'base_vertices': grad_base_vertices,
        }
        
        return grad_weights


# ============================================================
# Quantum Vertex ODE Function
# ============================================================
class QuantumVertexODE:
    """
    ODE function that evolves vertex points through time.
    Uses learned weights for dynamics.
    """
    
    def __init__(self, num_vertices=12, vertex_dim=3, hidden_dim=128, seed=42):
        np.random.seed(seed)
        
        self.num_vertices = num_vertices
        self.vertex_dim = vertex_dim
        self.state_dim = num_vertices * vertex_dim
        
        # ODE dynamics network
        # Input: vertices (num_vertices * 3) + time (1)
        self.W_dyn1 = np.random.randn(self.state_dim + 1, hidden_dim) * np.sqrt(2.0 / (self.state_dim + 1))
        self.b_dyn1 = np.zeros(hidden_dim)
        
        self.W_dyn2 = np.random.randn(hidden_dim, hidden_dim) * np.sqrt(2.0 / hidden_dim)
        self.b_dyn2 = np.zeros(hidden_dim)
        
        self.W_dyn3 = np.random.randn(hidden_dim, self.state_dim) * 0.01
        self.b_dyn3 = np.zeros(self.state_dim)
        
        # Attention weights for vertex interactions.
        # These operate on each 3D vertex independently, not on the flattened state.
        self.W_attn_q = np.random.randn(self.vertex_dim, 64) * 0.01
        self.W_attn_k = np.random.randn(self.vertex_dim, 64) * 0.01
        self.W_attn_v = np.random.randn(self.vertex_dim, 64) * 0.01
        self.W_attn_out = np.random.randn(64, self.vertex_dim) * 0.01
        
        self.cache = {}
        
    def dynamics(self, t, state):
        """
        Compute dx/dt for vertex evolution.
        
        Args:
            t: current time (scalar)
            state: (state_dim,) flattened vertex positions
        Returns:
            d_state/dt: (state_dim,)
        """
        state = state.reshape(-1)
        
        # Reshape to (num_vertices, 3)
        vertices = state.reshape(self.num_vertices, self.vertex_dim)
        
        # Attention mechanism
        Q = vertices @ self.W_attn_q  # (num_vertices, 64)
        K = vertices @ self.W_attn_k
        V = vertices @ self.W_attn_v
        
        # Attention scores
        attn_scores = Q @ K.T / np.sqrt(64)  # (num_vertices, num_vertices)
        attn_weights = softmax(attn_scores)
        attn_out = attn_weights @ V  # (num_vertices, 64)
        attn_vertices = attn_out @ self.W_attn_out  # (num_vertices, 3)
        
        # Combine state with time
        combined = np.concatenate([state, [t]])
        
        # Dynamics network
        h1 = relu(combined @ self.W_dyn1 + self.b_dyn1)
        h2 = relu(h1 @ self.W_dyn2 + self.b_dyn2)
        dynamics = h2 @ self.W_dyn3 + self.b_dyn3
        
        # Add attention contribution
        attn_flat = attn_vertices.flatten()
        dynamics = dynamics + attn_flat * 0.1
        
        return dynamics

    def dynamics_batch(self, t, state_batch):
        """
        Batched ODE dynamics.

        Args:
            t: current time (scalar)
            state_batch: (batch, state_dim)
        Returns:
            d_state/dt: (batch, state_dim)
        """
        vertices = state_batch.reshape(-1, self.num_vertices, self.vertex_dim)

        Q = vertices @ self.W_attn_q
        K = vertices @ self.W_attn_k
        V = vertices @ self.W_attn_v

        attn_scores = np.matmul(Q, np.swapaxes(K, 1, 2)) / np.sqrt(64.0)
        attn_weights = softmax(attn_scores, axis=2)
        attn_out = attn_weights @ V
        attn_vertices = attn_out @ self.W_attn_out

        time_column = np.full((state_batch.shape[0], 1), t)
        combined = np.concatenate([state_batch, time_column], axis=1)

        h1 = relu(combined @ self.W_dyn1 + self.b_dyn1)
        h2 = relu(h1 @ self.W_dyn2 + self.b_dyn2)
        dynamics = h2 @ self.W_dyn3 + self.b_dyn3
        dynamics += attn_vertices.reshape(state_batch.shape[0], -1) * 0.1

        return dynamics

    def solve_batch(self, initial_state_batch, t_span=(0, 1.5), num_steps=16):
        """
        Solve the ODE for a batch with fixed-step RK4.

        Args:
            initial_state_batch: (batch, state_dim)
            t_span: (t0, tf) time span
            num_steps: number of trajectory samples
        Returns:
            trajectory: (num_steps, batch, state_dim)
        """
        t_eval = np.linspace(t_span[0], t_span[1], num_steps)
        trajectory = np.empty((num_steps, initial_state_batch.shape[0], self.state_dim))
        trajectory[0] = initial_state_batch

        for i in range(1, num_steps):
            t = t_eval[i - 1]
            dt = t_eval[i] - t_eval[i - 1]
            state = trajectory[i - 1]

            k1 = self.dynamics_batch(t, state)
            k2 = self.dynamics_batch(t + 0.5 * dt, state + 0.5 * dt * k1)
            k3 = self.dynamics_batch(t + 0.5 * dt, state + 0.5 * dt * k2)
            k4 = self.dynamics_batch(t + dt, state + dt * k3)

            trajectory[i] = state + (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

        return trajectory


# ============================================================
# Quantum 3D Vertex Classifier
# ============================================================
class Quantum3DClassifier:
    """
    Complete classifier: Vertex Generation -> ODE Evolution -> Classification
    All from scratch with numpy and solve_ivp.
    """
    
    def __init__(self, input_size=784, num_vertices=12, num_classes=10,
                 batch_size=100, t_span=(0, 1.5), num_time_steps=16, seed=42):
        np.random.seed(seed)
        
        self.num_classes = num_classes
        self.num_vertices = num_vertices
        self.batch_size = batch_size
        self.t_span = t_span
        self.num_time_steps = num_time_steps
        
        # Vertex generator
        self.vertex_gen = QuantumVertexGenerator(
            input_size=input_size,
            num_vertices=num_vertices,
            batch_size=batch_size,
            seed=seed
        )
        
        # ODE solver
        self.ode = QuantumVertexODE(
            num_vertices=num_vertices,
            vertex_dim=3,
            hidden_dim=128,
            seed=seed + 1
        )
        
        # Trajectory encoder
        self.W_traj1 = np.random.randn(num_vertices * 3 * 3, 256) * np.sqrt(2.0 / (num_vertices * 3 * 3))
        self.b_traj1 = np.zeros(256)
        self.W_traj2 = np.random.randn(256, 128) * np.sqrt(2.0 / 256)
        self.b_traj2 = np.zeros(128)
        
        # Vertex statistics encoder
        self.W_stat1 = np.random.randn(num_vertices * 3 * 5, 128) * np.sqrt(2.0 / (num_vertices * 3 * 5))
        self.b_stat1 = np.zeros(128)
        self.W_stat2 = np.random.randn(128, 64) * np.sqrt(2.0 / 128)
        self.b_stat2 = np.zeros(64)
        
        # Time embedding
        self.time_embed = np.random.randn(num_time_steps) * 0.01
        
        # Final classifier
        self.W_cls1 = np.random.randn(192 + num_time_steps, 256) * np.sqrt(2.0 / (192 + num_time_steps))
        self.b_cls1 = np.zeros(256)
        self.W_cls2 = np.random.randn(256, 128) * np.sqrt(2.0 / 256)
        self.b_cls2 = np.zeros(128)
        self.W_cls3 = np.random.randn(128, num_classes) * 0.01
        self.b_cls3 = np.zeros(num_classes)
        
        self.cache = {}
        
    def compute_trajectory_features(self, trajectory):
        """Extract features from ODE trajectory."""
        # trajectory: (num_steps, batch, state_dim)
        initial = trajectory[0]           # (batch, state_dim)
        final = trajectory[-1]            # (batch, state_dim)
        mean_traj = trajectory.mean(axis=0)  # (batch, state_dim)
        
        return np.concatenate([initial, final, mean_traj], axis=1)
    
    def compute_vertex_statistics(self, vertices):
        """Compute statistics from vertex positions."""
        # vertices: (batch, num_vertices, 3)
        mean = vertices.mean(axis=1)      # (batch, 3)
        std = vertices.std(axis=1)        # (batch, 3)
        max_v = vertices.max(axis=1)      # (batch, 3)
        min_v = vertices.min(axis=1)      # (batch, 3)
        
        stats = np.concatenate([mean, std, max_v, min_v, max_v - min_v], axis=1)  # (batch, 15)
        
        # Expand and flatten
        stats = np.repeat(stats[:, np.newaxis, :], self.num_vertices, axis=1)
        return stats.reshape(stats.shape[0], -1)  # (batch, num_vertices * 15)
    
    def forward(self, X, return_trajectory=False):
        """
        Forward pass through the entire network.
        
        Args:
            X: (batch, 784) normalized images
            return_trajectory: if True, return ODE trajectory for visualization
        Returns:
            logits: (batch, num_classes)
            trajectory: optional (num_steps, batch, state_dim)
        """
        batch_size = X.shape[0]
        
        # Generate vertices
        vertices = self.vertex_gen.forward(X)  # (batch, num_vertices, 3)
        vertices_flat = vertices.reshape(batch_size, -1)  # (batch, state_dim)
        
        trajectory = self.ode.solve_batch(
            vertices_flat,
            t_span=self.t_span,
            num_steps=self.num_time_steps
        )
        
        # Compute trajectory features
        traj_features = self.compute_trajectory_features(trajectory)  # (batch, state_dim * 3)
        
        # Encode trajectory
        h_traj = relu(traj_features @ self.W_traj1 + self.b_traj1)
        h_traj = relu(h_traj @ self.W_traj2 + self.b_traj2)  # (batch, 128)
        
        # Get final vertices
        final_vertices = trajectory[-1].reshape(batch_size, self.num_vertices, 3)  # (batch, num_v, 3)
        
        # Compute vertex statistics
        vertex_stats = self.compute_vertex_statistics(final_vertices)  # (batch, num_v * 15)
        
        # Encode statistics
        h_stat = relu(vertex_stats @ self.W_stat1 + self.b_stat1)
        h_stat = relu(h_stat @ self.W_stat2 + self.b_stat2)  # (batch, 64)
        
        # Combine all features
        combined = np.concatenate([h_traj, h_stat, np.tile(self.time_embed, (batch_size, 1))], axis=1)
        
        # Classifier
        h_cls = relu(combined @ self.W_cls1 + self.b_cls1)
        h_cls = relu(h_cls @ self.W_cls2 + self.b_cls2)
        logits = h_cls @ self.W_cls3 + self.b_cls3
        
        if return_trajectory:
            return logits, trajectory, vertices
        return logits
    
    def predict(self, X):
        """Predict class labels."""
        predictions = []
        for start in range(0, len(X), self.batch_size):
            logits = self.forward(X[start:start + self.batch_size])
            predictions.append(np.argmax(softmax(logits), axis=1))
        return np.concatenate(predictions, axis=0)
    
    def score(self, X, y):
        """Compute accuracy."""
        predictions = self.predict(X)
        return np.mean(predictions == y)
    
    def compute_loss(self, X, y_true):
        """Compute cross-entropy loss."""
        logits = self.forward(X)
        probabilities = softmax(logits)
        
        # One-hot encode labels
        y_onehot = np.zeros((len(y_true), self.num_classes))
        y_onehot[np.arange(len(y_true)), y_true] = 1.0
        
        # Cross-entropy loss
        loss = -np.sum(y_onehot * np.log(probabilities + 1e-9)) / len(y_true)
        
        return loss, logits


# ============================================================
# Visualization
# ============================================================
def visualize_vertices(model, X_test, y_test, num_samples=8):
    """Visualize generated 3D vertices and ODE evolution."""
    
    indices = np.arange(num_samples)
    X_samples = X_test[indices]
    y_samples = y_test[indices]
    
    # Forward pass with trajectory
    logits, trajectory, initial_vertices = model.forward(X_samples, return_trajectory=True)
    
    predictions = np.argmax(softmax(logits), axis=1)
    
    num_steps = trajectory.shape[0]
    time_axis = np.linspace(model.t_span[0], model.t_span[1], num_steps)
    
    # Create figure
    fig = plt.figure(figsize=(18, 13))
    
    # Plot sample images and initial vertices
    for i in range(min(num_samples, 8)):
        # Image
        ax1 = fig.add_subplot(4, 6, i*2 + 1)
        ax1.imshow(X_samples[i].reshape(28, 28), cmap='gray')
        ax1.set_title(f'True: {y_samples[i]}\nPred: {predictions[i]}', fontsize=10)
        ax1.axis('off')
        
        # Initial 3D vertices
        ax2 = fig.add_subplot(4, 6, i*2 + 2, projection='3d')
        v_init = initial_vertices[i]
        ax2.scatter(v_init[:, 0], v_init[:, 1], v_init[:, 2], 
                   c=range(12), cmap='viridis', s=100, alpha=0.8)
        ax2.set_title('Initial Vertices', fontsize=8)
    
    # Plot evolution of one sample
    ax3 = fig.add_subplot(4, 6, (13, 14), projection='3d')
    sample_idx = 0
    colors = plt.cm.plasma(np.linspace(0, 1, num_steps))
    
    step_stride = max(1, num_steps // 4)
    for t_idx in range(0, num_steps, step_stride):
        v_traj = trajectory[t_idx, sample_idx].reshape(model.num_vertices, 3)
        t_value = time_axis[t_idx]
        ax3.scatter(v_traj[:, 0], v_traj[:, 1], v_traj[:, 2], 
                   label=f't={t_value:.2f}', s=50, alpha=0.8, c=[colors[t_idx]])
    ax3.legend()
    ax3.set_title('Vertex Evolution (Sample 0)')
    ax3.set_xlabel('X')
    ax3.set_ylabel('Y')
    ax3.set_zlabel('Z')
    
    # Full 3D trajectory lines for selected vertices
    ax4 = fig.add_subplot(4, 6, 15)
    sample_traj = trajectory[:, sample_idx].reshape(num_steps, model.num_vertices, 3)
    for v in range(0, model.num_vertices, 3):
        z_vals = sample_traj[:, v, 2]
        ax4.plot(time_axis, z_vals, label=f'Vertex {v}')
    ax4.plot(time_axis, sample_traj[:, :, 2].mean(axis=1), color='black', linewidth=2, label='Mean Z')
    ax4.set_xlabel('Time')
    ax4.set_ylabel('Z Position')
    ax4.set_title('Vertical Motion Over Time')
    ax4.legend(fontsize=8)
    ax4.grid(True, alpha=0.3)
    
    # Phase plot (vertex 0 trajectory in 3D)
    ax5 = fig.add_subplot(4, 6, 16, projection='3d')
    v0_traj = trajectory[:, sample_idx, :3]
    
    for i in range(num_steps - 1):
        ax5.plot(v0_traj[i:i+2, 0], v0_traj[i:i+2, 1], v0_traj[i:i+2, 2], 
                color=colors[i], linewidth=2)
    ax5.scatter(v0_traj[0, 0], v0_traj[0, 1], v0_traj[0, 2], 
               c='green', s=100, label='Start', marker='o')
    ax5.scatter(v0_traj[-1, 0], v0_traj[-1, 1], v0_traj[-1, 2], 
               c='red', s=100, label='End', marker='s')
    ax5.legend()
    ax5.set_title('Vertex 0 Trajectory')
    ax5.set_xlabel('X')
    ax5.set_ylabel('Y')
    ax5.set_zlabel('Z')
    
    # 3D path lines for several vertices
    ax6 = fig.add_subplot(4, 6, (17, 18), projection='3d')
    vertex_ids = [0, 3, 6, 9]
    line_colors = plt.cm.viridis(np.linspace(0.1, 0.9, len(vertex_ids)))
    for color, v in zip(line_colors, vertex_ids):
        coords = sample_traj[:, v, :]
        ax6.plot(coords[:, 0], coords[:, 1], coords[:, 2], color=color, linewidth=2, label=f'Vertex {v}')
        ax6.scatter(coords[0, 0], coords[0, 1], coords[0, 2], color=color, s=25)
        ax6.scatter(coords[-1, 0], coords[-1, 1], coords[-1, 2], color=color, s=55, marker='s')
    ax6.set_title('Selected Vertex Paths')
    ax6.set_xlabel('X')
    ax6.set_ylabel('Y')
    ax6.set_zlabel('Z')
    ax6.legend(fontsize=8)
    
    # Vertical spread band
    ax7 = fig.add_subplot(4, 6, 21)
    z_values = sample_traj[:, :, 2]
    z_min = z_values.min(axis=1)
    z_max = z_values.max(axis=1)
    z_mean = z_values.mean(axis=1)
    ax7.fill_between(time_axis, z_min, z_max, color='skyblue', alpha=0.35, label='Z range')
    ax7.plot(time_axis, z_mean, color='navy', linewidth=2, label='Mean Z')
    ax7.set_title('Vertical Envelope')
    ax7.set_xlabel('Time')
    ax7.set_ylabel('Z Position')
    ax7.legend(fontsize=8)
    ax7.grid(True, alpha=0.3)
    
    # Center-of-mass motion
    ax8 = fig.add_subplot(4, 6, 22)
    center_of_mass = sample_traj.mean(axis=1)
    ax8.plot(time_axis, center_of_mass[:, 0], label='COM X')
    ax8.plot(time_axis, center_of_mass[:, 1], label='COM Y')
    ax8.plot(time_axis, center_of_mass[:, 2], label='COM Z', linewidth=2)
    ax8.set_title('Center of Mass Motion')
    ax8.set_xlabel('Time')
    ax8.set_ylabel('Position')
    ax8.legend(fontsize=8)
    ax8.grid(True, alpha=0.3)
    
    plt.tight_layout()
    plt.savefig('quantum_vertices_numpy.png', dpi=150, bbox_inches='tight')
    plt.show()


# ============================================================
# Main
# ============================================================
if __name__ == "__main__":
    # Configuration
    NUM_VERTICES = 12
    NUM_CLASSES = 10
    T_SPAN = (0, 1.5)
    NUM_TIME_STEPS = 16
    EVAL_BATCH_SIZE = 100
    VIS_SAMPLES = 8
    
    print("=" * 60)
    print("Quantum 3D Vertex Classifier Prototype (NumPy + batched RK4)")
    print("=" * 60)
    print(f"Vertices per sample: {NUM_VERTICES}")
    print(f"t_span: {T_SPAN}")
    print(f"Time steps: {NUM_TIME_STEPS}")
    print(f"Eval batch size: {EVAL_BATCH_SIZE}")
    print(f"Visualization samples: {VIS_SAMPLES}")
    print("Mode: forward-pass visualization and diagnostics only")
    print("Trainable reference: run ex04.py")
    print("=" * 60)
    
    # Create model
    model = Quantum3DClassifier(
        input_size=784,
        num_vertices=NUM_VERTICES,
        num_classes=NUM_CLASSES,
        batch_size=EVAL_BATCH_SIZE,
        t_span=T_SPAN,
        num_time_steps=NUM_TIME_STEPS,
        seed=42
    )
    
    print("\nModel created successfully!")
    print(f"Total parameters: ~{sum([p.size for p in [model.vertex_gen.W1, model.vertex_gen.W2, model.vertex_gen.W3]])}")
    print("=" * 60)
    
    # Prototype evaluation
    print("\n" + "=" * 60)
    print("PROTOTYPE EVALUATION")
    print("=" * 60)
    final_test_acc = model.score(X_test, y_test) * 100
    print(f"Random-weight Test Accuracy: {final_test_acc:.2f}%")
    sample_loss, _ = model.compute_loss(X_test[:EVAL_BATCH_SIZE], y_test[:EVAL_BATCH_SIZE])
    print(f"Sample Batch Loss: {sample_loss:.4f}")
    print("=" * 60)
    
    # Visualize
    visualize_vertices(model, X_test, y_test, num_samples=VIS_SAMPLES)
    
    # Save model weights
    print("\nSaving prototype weights...")
    weights = {
        'vertex_gen': {
            'W1': model.vertex_gen.W1,
            'b1': model.vertex_gen.b1,
            'W2': model.vertex_gen.W2,
            'b2': model.vertex_gen.b2,
            'W3': model.vertex_gen.W3,
            'b3': model.vertex_gen.b3,
            'base_vertices': model.vertex_gen.base_vertices,
            'W_rot': model.vertex_gen.W_rot,
            'b_rot': model.vertex_gen.b_rot,
            'W_scale': model.vertex_gen.W_scale,
            'b_scale': model.vertex_gen.b_scale,
            'W_trans': model.vertex_gen.W_trans,
            'b_trans': model.vertex_gen.b_trans,
        },
        'ode': {
            'W_dyn1': model.ode.W_dyn1,
            'b_dyn1': model.ode.b_dyn1,
            'W_dyn2': model.ode.W_dyn2,
            'b_dyn2': model.ode.b_dyn2,
            'W_dyn3': model.ode.W_dyn3,
            'b_dyn3': model.ode.b_dyn3,
        },
        'classifier': {
            'W_traj1': model.W_traj1,
            'b_traj1': model.b_traj1,
            'W_traj2': model.W_traj2,
            'b_traj2': model.b_traj2,
            'W_stat1': model.W_stat1,
            'b_stat1': model.b_stat1,
            'W_stat2': model.W_stat2,
            'b_stat2': model.b_stat2,
            'W_cls1': model.W_cls1,
            'b_cls1': model.b_cls1,
            'W_cls2': model.W_cls2,
            'b_cls2': model.b_cls2,
            'W_cls3': model.W_cls3,
            'b_cls3': model.b_cls3,
        }
    }
    np.savez('quantum_classifier_weights.npz', **weights)
    print("Weights saved to quantum_classifier_weights.npz")
