import copy
from typing import Tuple

import torch
from torch_geometric.data import Batch, Data

from ltsgns_mp.algorithms.cnp import CNP
from ltsgns_mp.architectures.loss_functions.mse import mse
from ltsgns_mp.architectures.simulators.cnp_simulator import CNPSimulator
from ltsgns_mp.envs import Env
from ltsgns_mp.envs.train_iterator.cnp_train_iterator import CNPTrainBatch
from ltsgns_mp.util import keys
from ltsgns_mp.util.graph_input_output_util import remove_edge_distances
from ltsgns_mp.util.graph_input_output_util import add_distances_from_positions, node_type_mask
from ltsgns_mp.util.own_types import ValueDict, ConfigDict


class CNP_MP(CNP):
    def __init__(self, config: ConfigDict, simulator: CNPSimulator,
                 env: Env, optimizer: torch.optim.Optimizer, loading_config: ConfigDict, device: str):
        super().__init__(config, simulator, env, optimizer, loading_config, device)

    def _single_train_step(self, batch: CNPTrainBatch) -> torch.Tensor:
        # unpack batch
        context_batch = batch.context_batch
        target_batch = batch.target_batch
        z = self.simulator.compute_z(context_batch)
        target_batch = self.simulator.add_z_to_batch(target_batch, z)
        anchor_index = int(context_batch.anchor_index)
        # is usually velocities, or accelerations if second order dynamics are used
        trajectory_prediction = self.simulator(target_batch, initial_time=anchor_index)
        gth_positions = torch.permute(target_batch.context_node_positions, (0, 2, 1, 3))
        # only 1 z sample
        gth_positions = gth_positions[0]
        loss = mse(trajectory_prediction, gth_positions)
        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:

        """
        with torch.no_grad():
            context_batch, target_data = self.extract_cnp_context(data)
            anchor_index = int(target_data[keys.ANCHOR_INDICES])

            use_collider = keys.CONTEXT_COLLIDER_POSITIONS in data
            history_positions = []
            for hist_idx in range(anchor_index - 1,
                                  anchor_index - self.config.train_iterator.get("history_length", 0) - 1, -1):
                if hist_idx < 0:
                    hist_idx = 0
                hist_pos = data[keys.CONTEXT_NODE_POSITIONS][0, hist_idx]
                if use_collider:
                    hist_collider = data[keys.CONTEXT_COLLIDER_POSITIONS][0, hist_idx]
                    hist_pos = torch.cat([hist_pos, hist_collider], dim=0)
                history_positions.insert(0, hist_pos)
            history_vels = []
            current_pos = data.pos
            for hist_idx in range(len(history_positions) - 1, -1, -1):
                prev_pos = history_positions[hist_idx]
                vel = current_pos - prev_pos
                history_vels.insert(0, vel)
                # order: from oldest to newest
                current_pos = prev_pos
            if self.config.train_iterator.get("history_length", 0) > 0:
                history_vels_tensor = torch.stack(history_vels, dim=1)
                history_vels_tensor = history_vels_tensor.view(history_vels_tensor.shape[0], -1)
                target_data.x = torch.cat([target_data.x, history_vels_tensor], dim=1)
                target_data.x_description.extend(["history_velocities"] * history_vels_tensor.shape[1])

            z = self.simulator.compute_z(context_batch)
            target_data = self.simulator.add_z_to_batch(target_data, z)
            target_data = remove_edge_distances(target_data)
            target_data = add_distances_from_positions(target_data, self.config.train_iterator.add_euclidian_distance)
            target_data = Batch.from_data_list([target_data])

            trajectory_prediction = self.simulator(target_data,
                                                   initial_time=anchor_index,
                                                   )
            # permute to have dim order [num_timesteps, num_nodes, world_dim]
            trajectory_prediction = torch.permute(trajectory_prediction, (1, 0, 2))
            mesh_positions = target_data[keys.CONTEXT_NODE_POSITIONS][0]
            # get positions up to the first predicted step (including anchor step)
            trajectory_prediction[:anchor_index + 1] = mesh_positions[:anchor_index + 1]
        return trajectory_prediction, {}  # Never visualize additional information

    @property
    def simulator(self) -> CNPSimulator:
        if self._simulator is None:
            raise ValueError("Simulator not set")
        return self._simulator
