import gymnasium as gym
import numpy as np
from gymnasium import spaces
from collections import deque
from typing import Tuple
import sys
import os
import matplotlib.pyplot as plt
import queue

os.environ["OMP_NUM_THREADS"] = "1"
os.environ["MKL_NUM_THREADS"] = "1"

current_dir = os.path.dirname(os.path.abspath("__file__"))
parent_dir = os.path.abspath(os.path.join(current_dir, os.pardir))
sys.path.append(parent_dir)
from CelerisLab import FlowField
from CelerisLab import utils

config_cuda = utils.load_cuda_config(
    os.path.join(parent_dir, "configs", "config_cuda.json")
)
config_field = utils.load_flow_field_config(
    os.path.join(parent_dir, "configs", "config_flowfield.json")
)

S_DIM, A_DIM = 12, 3
U0 = config_field.velocity
T0 = 1000
SAMPLE_INTERVAL = 800
FIFO_LEN = 120
CONV_LEN = 60
MAX_STEPS = 500
if config_field.data_type == "FP32":
    DATA_TYPE = np.float32
else:
    raise ValueError(f"Unsupported data type {config_field.data_type}.")


class CustomEnv(gym.Env):
    """Custom Environment that follows gym interface."""

    metadata = {"render_modes": ["human"], "render_fps": T0 / SAMPLE_INTERVAL}

    def __init__(self, device_id=0):
        super().__init__()
        self.action_space = spaces.Box(low=-1, high=1, shape=(A_DIM,), dtype=DATA_TYPE)
        self.observation_space = spaces.Box(
            low=-7, high=7, shape=(S_DIM,), dtype=DATA_TYPE
        )
        self.fifo_states = deque(maxlen=FIFO_LEN)
        self.save_states = deque(maxlen=FIFO_LEN)
        self.force_norm_fact = 1.0
        self.sens_norm_fact = 1.0
        self.sens_deviation = np.zeros(6, dtype=DATA_TYPE)
        self.reward_cd = 0.0
        self.reward_cl = 0.0
        self.reward_sim = 0.0
        self.current_step = 0

        self.flow_field = FlowField(config_field, config_cuda, device_id)
        L0 = 20
        U0 = config_field.velocity
        NX = self.flow_field.FIELD_SHAPE[0]
        NY = self.flow_field.FIELD_SHAPE[1]
        center: Tuple[float, float, float] = (20 * L0, (NY - 1) / 2, 0)
        self.flow_field.add_cylinder(center, L0 / 2)
        center: Tuple[float, float, float] = (21.3 * L0, (NY - 1) / 2 + 0.75 * L0, 0)
        self.flow_field.add_cylinder(center, L0 / 2)
        center: Tuple[float, float, float] = (21.3 * L0, (NY - 1) / 2 - 0.75 * L0, 0)
        self.flow_field.add_cylinder(center, L0 / 2)
        center: Tuple[float, float, float] = (30 * L0, (NY - 1) / 2 + 3 * L0, 0)
        self.flow_field.add_sensor(center, L0 / 4)
        center: Tuple[float, float, float] = (30 * L0, (NY - 1) / 2, 0)
        self.flow_field.add_sensor(center, L0 / 4)
        center: Tuple[float, float, float] = (30 * L0, (NY - 1) / 2 - 3 * L0, 0)
        self.flow_field.add_sensor(center, L0 / 4)
        self.flow_field.run(int(4*NX/U0), np.zeros(6, dtype=DATA_TYPE))

        for i in range(FIFO_LEN):
            self.flow_field.run(SAMPLE_INTERVAL, np.zeros(6, dtype=DATA_TYPE))
            self.fifo_states.append(self.flow_field.obs.copy())
        
        self.save_states = self.fifo_states.copy()
        self.flow_field.get_ddf()
        self.flow_field.save_ddf()
            

    def step(self, action):
        assert self.action_space.contains(action), "%r (%s) invalid" % (
            action,
            type(action),
        )

        result_queue = queue.Queue()

        def run_flow_field(action):
            self.flow_field.context.push()
            U0 = config_field.velocity
            try:
                temp = np.zeros(6, dtype=DATA_TYPE)
                temp[0:3] = np.array((action*5+[0,-2.5,2.5])*U0, dtype=DATA_TYPE)
                self.flow_field.run(SAMPLE_INTERVAL, temp)
            finally:
                self.fifo_states.append(self.flow_field.obs.copy())
                self.flow_field.context.pop()

        def proc_data():
            U0 = config_field.velocity
            NY = self.flow_field.FIELD_SHAPE[1]
            L0 = 20
            states = np.array(self.fifo_states)
            forces = states[-1, 0:6] / (L0*U0*U0)
            cd = (forces[0] + forces[2] + forces[4])
            cl = (forces[1] + forces[3] + forces[5])
            sens = states[-1, 6:12] / 78 / U0

            def theo_velo(y):
                NY = self.flow_field.FIELD_SHAPE[1]
                U0 = config_field.velocity
                yy = (y - 0.5 * (NY - 1)) / (NY - 2.0)
                u = U0 * 1.5 * (1 - 4 * yy * yy)
                return u
            
            similarities = 0.0
            sens_pos = np.array([(NY - 1) / 2 + 2 * L0, (NY - 1) / 2, (NY - 1) / 2 - 2 * L0])
            for i in range(3):
                u = theo_velo(sens_pos[i])*78
                similarities += np.exp(-4*np.abs(states[-1, 2*i+6] - u))/6 + np.exp(-8*np.abs(states[-1, 2*i+7] - 0))/6

            self.reward_cd = np.exp(-np.abs(cd))
            self.reward_cl = np.exp(-np.abs(cl))
            # self.reward_cl = cd * 10.0
            # reward_sim = np.exp(2 * (similarities - 1))
            self.reward_sim = similarities
            reward = np.clip(0.6 * self.reward_cd + 0.4 * self.reward_cl + 0.0 * self.reward_sim, 0, 1.0)
            # barrier.wait()
            result_queue.put((np.hstack([forces, sens]), reward))

        run_flow_field(action)
        proc_data()
        observation, reward = result_queue.get()

        truncated = bool(np.any(observation > 7) or np.any(observation < -7))
        observation = np.clip(observation, -7, 7)
        terminated = self.current_step >= MAX_STEPS
        self.current_step += 1
        return observation, float(reward), terminated, truncated, {}
    
    def reset(self, seed=None):
        self.flow_field.apply_ddf()
        self.current_step = 0
        self.fifo_states = self.save_states.copy()
        return np.zeros(S_DIM, dtype=np.float32), {}
    
    def render(self, mode="human"):
        NX = self.flow_field.FIELD_SHAPE[0]
        NY = self.flow_field.FIELD_SHAPE[1]
        self.flow_field.get_ddf()
        ddf_plot = self.flow_field.ddf.copy().reshape((9, NY, NX)).transpose(2, 1, 0)
        ux = (ddf_plot[:, :, 1] + ddf_plot[:, :, 5] + ddf_plot[:, :, 8] - ddf_plot[:, :, 3] - ddf_plot[:, :, 6] - ddf_plot[:, :, 7]) / U0
        uy = (ddf_plot[:, :, 2] + ddf_plot[:, :, 5] + ddf_plot[:, :, 6] - ddf_plot[:, :, 4] - ddf_plot[:, :, 7] - ddf_plot[:, :, 8]) / U0
        speed = np.sqrt(ux**2 + uy**2)
        plt.figure(figsize=(10, 5))
        plt.imshow(speed.T, origin='lower', cmap='viridis', extent=[0, NX, 0, NY])
        plt.colorbar(label='Speed')
        plt.title('Scalar Velocity Field')
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.tight_layout()
        plt.show()

    def save_field(self, filename):
        NX = self.flow_field.FIELD_SHAPE[0]
        NY = self.flow_field.FIELD_SHAPE[1]
        self.flow_field.get_ddf()
        ddf_plot = self.flow_field.ddf.copy().reshape((9, NY, NX)).transpose(2, 1, 0)
        flag_plot = self.flow_field.flag.copy().reshape((NY, NX)).transpose(1, 0)
        ux = (ddf_plot[:, :, 1] + ddf_plot[:, :, 5] + ddf_plot[:, :, 8] - ddf_plot[:, :, 3] - ddf_plot[:, :, 6] - ddf_plot[:, :, 7]) / U0
        uy = (ddf_plot[:, :, 2] + ddf_plot[:, :, 5] + ddf_plot[:, :, 6] - ddf_plot[:, :, 4] - ddf_plot[:, :, 7] - ddf_plot[:, :, 8]) / U0
        with open(os.path.join(parent_dir, "output", filename), "w") as f:
            f.write("Title= \"LBM 2D\"\r\n")
            f.write("VARIABLES= \"X\",\"Y\",\"flag\",\"U\",\"V\",\r\n")
            f.write(f"ZONE T= \"BOX\",I= {NX},J= {NY},F=POINT\r\n")
            for j in range(NY):
                for i in range(NX):
                    f.write(f"{i},{j},{flag_plot[i, j]},{ux[i, j]},{uy[i, j]}\r\n")

    def close(self):
        self.flow_field.__del__()