import copy
from typing import Tuple

import torch
from torch_geometric.data import Batch, Data

from ltsgns_mp.algorithms.abstract_algorithm import AbstractAlgorithm
from ltsgns_mp.algorithms.util import _update_external_state
from ltsgns_mp.architectures.loss_functions.mse import mse
from ltsgns_mp.envs.train_iterator.step_train_iterator import StepTrainBatch
from ltsgns_mp.util import keys
from ltsgns_mp.util.graph_input_output_util import recompute_external_edges, add_and_update_node_features
from ltsgns_mp.util.graph_input_output_util import add_distances_from_positions, node_type_mask
from ltsgns_mp.util.own_types import ValueDict


class MGN(AbstractAlgorithm):
    def _single_train_step(self, batch: StepTrainBatch) -> torch.Tensor:
        # unpack batch
        batch = batch.batch
        # is usually velocities, or accelerations if second order dynamics are used
        predicted_dynamics = self.simulator(batch)
        gth_dynamics = batch.y
        loss = mse(predicted_dynamics, gth_dynamics)
        self._apply_loss(loss)
        return loss.detach().item()

    def predict_trajectory(self, data: Data, visualize: bool = False, eval_only: bool = False) -> Tuple[
        torch.Tensor, ValueDict]:
        """

        Args:
            data:
            visualize:
                Because in LTSGNS_MP we sometimes have visualizations of the ELBO or the latent space which we
                only want to log in the visualization epochs (every 50-ish epochs)
            eval_only: Not relevant here, but in LTSGNS. However, if something should be different if we are in eval only

        Returns:

        """
        batch_index = 0  # todo we currently only have a single batch

        node_mask = node_type_mask(data, keys.MESH, as_index=True)  # assume consistent collider indices
        mesh_positions = data[keys.CONTEXT_NODE_POSITIONS][batch_index]

        # get positions up to the first predicted step (including anchor step)
        first_step = data[keys.ANCHOR_INDICES]
        output_trajectories = [mesh_positions[i] for i in range(first_step + 1)]

        # predict the remaining steps from the anchor graph onwards in the future
        trajectory_length = mesh_positions.shape[0]

        # add the velocity features to the data if necessary and update the node features
        data = add_and_update_node_features(data, self.config.second_order_dynamics)
        with torch.no_grad():
            for current_step in range(first_step + 1, trajectory_length):
                # update collider positions and potentially other external (sensory) information such as point_clouds
                if len(output_trajectories) > 1:
                    prev_pos = output_trajectories[-2]
                else:
                    prev_pos = output_trajectories[-1]
                data = self._update_external_state(batch_index, current_step, data, prev_pos)

                # recompute *all* edges, remove edge distances in preparation for next iteration
                data = recompute_external_edges(data, self.env.config, self._device)
                # compute and add relative distances
                # count edge types
                data = add_distances_from_positions(data, self.config.train_iterator.add_euclidian_distance)

                # predict velocities/accelerations
                predicted_dynamics = self.simulator(data)

                # update positions
                if self.config.second_order_dynamics:
                    velocities_indices = [index for index, value in enumerate(data.x_description) if
                                          value == keys.VELOCITIES]
                    all_vel = data.x[:, velocities_indices]
                    mesh_vel = all_vel[node_mask]
                    mesh_vel += predicted_dynamics
                    data[keys.POSITIONS][node_mask] += mesh_vel
                else:
                    data[keys.POSITIONS][node_mask] += predicted_dynamics

                # add updated mesh positions to output trajectories
                new_positions = copy.deepcopy(data[keys.POSITIONS][node_mask].detach())
                output_trajectories.append(new_positions)
        # finalize output trajectories
        output_trajectories = torch.stack(output_trajectories, dim=0)
        return output_trajectories, {}  # Never visualize additional information

    def _update_external_state(self, batch_index: int, current_step: int, data: Data,
                               last_mesh_pos: torch.Tensor | None = None) -> Data:
        """
        Update the external state of the graph by updating the information of the collider. Also updates new x features
        if necessary.
        Args:
            batch_index: Index of the batch to evaluate. Currently only supports a single batch with index 0
            current_step: Current step in the trajectory
            data: Data object containing the anchor graph and auxiliary information about the external trajectory state

        Returns:

        """
        data = _update_external_state(batch_index, current_step, data, last_mesh_pos)

        return data



