#!/usr/bin/env python
# ./dump2cube_edge_corner.py -i 050Li3PS4-050LiI.lammpstrj -m 160 160 160 -cut 8
import numpy as np
import argparse
import re
import os
import time

description = """This is a test program"""
par = argparse.ArgumentParser(description=description)

par.add_argument('-i', '--trjfile', default="", required=True,
                 help='input trjfile')
par.add_argument('-m', '--mesh', default=(30, 30, 30), required=False,
                 nargs=3, type=int, help='mesh grid')
par.add_argument('-cut', '--cutoff', default=7, required=False,
                 type=float, help='mesh grid')

args = par.parse_args()
ang_borr = 0.5291772109217
dirname = os.path.dirname(__file__)


class LammpsTrj():
    def __init__(self):
        """
        trjファイルを読み込んで、原子数、lattice、ステップ数を格納
        """
        with open(args.trjfile) as o:
            data = o.read().split()
        self.atoms = int(data[7])  # 原子数
        data = np.array(data).reshape(-1, self.atoms*7+32)
        self.lattices = data[:, 17:23].astype(float)  # (step, 6)
        self.data = data[:, 32:]  # 座標データ
        self.steps = data.shape[0]
        self.mesh = args.mesh
        self.cutoff = args.cutoff

    def setLattice(self, lat):
        """
        latticeの形によって操作を分岐
        self.M, self.M_を作成
        """
        if lat.shape[0] == 6:
            M = np.array([[lat[1]-lat[0], 0, 0],
                          [0, lat[3]-lat[2], 0],
                          [0, 0, lat[5]-lat[4]]]).astype(float)
        if lat.shape[0] == 9:
            xlo_bound, xhi_bound, xy = lat[0, 0], lat[0, 1], lat[0, 2]
            ylo_bound, yhi_bound, xz = lat[1, 0], lat[1, 1], lat[1, 2]
            zlo_bound, zhi_bound, yz = lat[2, 0], lat[2, 1], lat[2, 2]
            xlo = xlo_bound - np.min([0.0, xy, xz, xy+xz])
            xhi = xhi_bound - np.max([0.0, xy, xz, xy+xz])
            ylo = ylo_bound - np.min([0.0, yz])
            yhi = yhi_bound - np.max([0.0, yz])
            zlo = zlo_bound
            zhi = zhi_bound
            lx = xhi - xlo
            ly = yhi - ylo
            lz = zhi - zlo
            a = lx
            b = np.sqrt(ly**2 + xy**2)
            c = np.sqrt(lz**2 + xz**2 + yz**2)
            alpha = np.arccos((xy*xz + ly*yz)/b/c)
            beta = np.arccos(xz/c)
            gamma = np.arccos(xy/b)
            v1 = [a, 0, 0]
            v2 = [b*np.cos(gamma), b*np.sin(gamma), 0]
            v3 = [c*np.cos(beta),
                  c*(np.cos(alpha)-np.cos(beta)*np.cos(gamma))/np.sin(gamma),
                  c*np.sqrt(1+2*np.cos(alpha)*np.cos(beta)*np.cos(gamma)
                            - np.cos(alpha)**2-np.cos(beta)**2
                            - np.cos(gamma)**2 / np.sin(gamma))]
            M = np.array([v1, v2, v3])
        return M

    def getOnestep(self, step):
        """
        引数のstepにおける座標を、原子ごとにself.elems(辞書)に格納する
        """
        step_data = self.data[step, :].reshape(self.atoms, -1)  # (atoms, 6)
        self.M = self.setLattice(self.lattices[step])
        self.M_ = np.linalg.inv(self.M)
        self.elems = {}
        for e in np.unique(step_data[:, 2]):
            d = step_data[step_data[:, 2] == e, 3:].astype(float)
            dxyz = d[:, 0:3]
            dxyz = dxyz @ self.M_
            dxyz = dxyz - np.floor(dxyz)
            d[:, 0:3] = dxyz
            self.elems[e] = d
        return self.elems

    def makeCube(self, trjfile):
        """
        P原子ごとに最も近い4つのI原子を選出し、Edge/Corner分類および回転を行う
        """
        self.li_coords_list = []
        self.i_coords_list = []
        self.i_zero_coords_list = []
        self.i_corner_coords_list = []
        self.i_edge_coords_list = []
        self.i_three_coords_list = []

        self.li_hist = None
        self.i_hist = None
        self.i_hist_zero = None
        self.i_hist_corner = None
        self.i_hist_edge = None
        self.i_hist_three = None

        count_p = 0
        I_S_CUTOFF = 4.7  # Edge/Corner判定用距離 (Angstrom)

        for step in range(self.steps):
            print(f"processing {step} step")
            self.elems = self.getOnestep(step)
            elems_ = {k: v.copy() for k, v in self.elems.items()}

            for p_data in elems_["P"]:

                # P原子に最も近い4つのI原子を特定
                i_diffs = elems_["I"][:, 0:3] - p_data[0:3]
                i_diffs = i_diffs - np.around(i_diffs)
                i_diffs_abs = i_diffs @ self.M
                distances = np.linalg.norm(i_diffs_abs, axis=1)

                sorted_indices = np.argsort(distances)
                closest_Is = elems_["I"][sorted_indices[:4]]
                closer_Is = elems_["I"][sorted_indices[:15]]

                nearest_I_coords = closest_Is[0]

                # 着目P原子と同じPS4を構成するS原子を取得
                s_data = elems_["S"][elems_["S"][:, 3] == p_data[3]]

                # --- 距離によるSの配置決定 ---
                si_diff = s_data[:, 0:3] - nearest_I_coords[0:3]
                si_diff = si_diff - np.around(si_diff)
                si_diff = np.linalg.norm(si_diff @ self.M, axis=1)

                n_S_idx = np.argmax(si_diff)
                s_indices = [0, 1, 2, 3]
                s_indices.remove(n_S_idx)
                n2_S_idx = s_indices[0]

                s_xyz = s_data[:, 0:3] - p_data[0:3]
                s_xyz = s_xyz - np.round(s_xyz)
                s_xyz = s_xyz @ self.M

                # --- 座標の回転行列を作成 ---
                theta = np.arctan2(s_xyz[n_S_idx][0], s_xyz[n_S_idx][1])
                self.Mat_z = np.array([[np.cos(-theta), np.sin(-theta), 0],
                                       [-np.sin(-theta), np.cos(-theta), 0],
                                       [0, 0, 1]])
                s_xyz = (self.Mat_z @ s_xyz.T).T

                theta2 = np.arctan2(s_xyz[n_S_idx][1], s_xyz[n_S_idx][2])
                self.Mat_x = np.array([[1, 0, 0],
                                       [0, np.cos(-theta2), np.sin(-theta2)],
                                       [0, -np.sin(-theta2), np.cos(-theta2)]])
                s_xyz = (self.Mat_x @ s_xyz.T).T

                theta3 = np.arctan2(s_xyz[n2_S_idx][0], s_xyz[n2_S_idx][1])
                self.Mat_z2 = np.array([[np.cos(-theta3), np.sin(-theta3), 0],
                                        [-np.sin(-theta3), np.cos(-theta3), 0],
                                        [0, 0, 1]])
                s_xyz = (self.Mat_z2 @ s_xyz.T).T
                self.s_xyz = s_xyz

                p_xyz = np.array([0, 0, 0])
                self.ps4_coord = np.vstack((p_xyz, s_xyz)) / ang_borr

                def rotate_coords(coords_fractional):
                    if len(coords_fractional) == 0:
                        return np.array([])
                    rot_xyz = coords_fractional[:, 0:3] - p_data[0:3]
                    rot_xyz = rot_xyz - np.round(rot_xyz)
                    rot_xyz = rot_xyz @ self.M
                    rot_xyz = (self.Mat_z @ rot_xyz.T).T
                    rot_xyz = (self.Mat_x @ rot_xyz.T).T
                    rot_xyz = (self.Mat_z2 @ rot_xyz.T).T
                    return rot_xyz

                # --- I原子のEdge/Corner判定と振り分け ---
                zero_Is = []
                edge_Is = []
                corner_Is = []
                three_Is = []
                other_Is = []

                for i_coord in closer_Is:
                    diff_si = s_data[:, 0:3] - i_coord[0:3]
                    diff_si = diff_si - np.around(diff_si)
                    diff_si_abs = diff_si @ self.M
                    dists = np.linalg.norm(diff_si_abs, axis=1)

                    # 距離がCUTOFF以下のS原子の数をカウント
                    close_S_count = np.sum(dists <= I_S_CUTOFF)

                    if close_S_count == 0:
                        zero_Is.append(i_coord)
                    elif close_S_count == 1:
                        corner_Is.append(i_coord)
                    elif close_S_count == 2:
                        edge_Is.append(i_coord)
                    elif close_S_count == 3:
                        three_Is.append(i_coord)
                    elif close_S_count > 3:
                        other_Is.append(i_coord)

                # 判定結果に基づいてそれぞれのリストに追加
                if len(zero_Is) > 0:
                    self.i_zero_coords_list.extend(
                        rotate_coords(np.array(zero_Is)).tolist())
                if len(edge_Is) > 0:
                    self.i_edge_coords_list.extend(
                        rotate_coords(np.array(edge_Is)).tolist())

                if len(corner_Is) > 0:
                    self.i_corner_coords_list.extend(
                        rotate_coords(np.array(corner_Is)).tolist())

                if len(three_Is) > 0:
                    self.i_three_coords_list.extend(
                        rotate_coords(np.array(three_Is)).tolist())

                # 全体のI用リストにも追加
                self.i_coords_list.extend(rotate_coords(closest_Is).tolist())

                count_p += 1

        print("Calculating Histograms...")
        volume = (self.cutoff*2)**3
        bounds = [[-(self.cutoff)/ang_borr, (self.cutoff)/ang_borr]] * 3

        # I (全体のI原子数で割ることで、Edge/Cornerの比率も反映させる)
        total_I_num = len(self.i_coords_list)

        if total_I_num > 0:
            # I (All)
            print("num_I_all: ", total_I_num)
            i_arr = np.array(self.i_coords_list) / ang_borr
            i_hist, _ = np.histogramdd(i_arr, bins=self.mesh, range=bounds)
            self.i_hist = i_hist.ravel() / total_I_num / volume

            # I (zero)
            if len(self.i_zero_coords_list) > 0:
                print("num_I_zero: ", len(self.i_zero_coords_list))
                i_arr_zero = np.array(self.i_zero_coords_list) / ang_borr
                i_hist_zero, _ = np.histogramdd(
                    i_arr_zero, bins=self.mesh, range=bounds)
                self.i_hist_zero = i_hist_zero.ravel() / total_I_num / volume

            # I (Corner)
            if len(self.i_corner_coords_list) > 0:
                print("num_I_corner: ", len(self.i_corner_coords_list))
                i_arr_corner = np.array(self.i_corner_coords_list) / ang_borr
                i_hist_corner, _ = np.histogramdd(
                    i_arr_corner, bins=self.mesh, range=bounds)
                self.i_hist_corner = i_hist_corner.ravel() / total_I_num / volume
            # I (Edge)
            if len(self.i_edge_coords_list) > 0:
                print("num_I_edge: ", len(self.i_edge_coords_list))
                i_arr_edge = np.array(self.i_edge_coords_list) / ang_borr
                i_hist_edge, _ = np.histogramdd(
                    i_arr_edge, bins=self.mesh, range=bounds)
                self.i_hist_edge = i_hist_edge.ravel() / total_I_num / volume
                
            # I (Three)
            if len(self.i_three_coords_list) > 0:
                print("num_I_three: ", len(self.i_three_coords_list))
                i_arr_three = np.array(self.i_three_coords_list) / ang_borr
                i_hist_three, _ = np.histogramdd(
                    i_arr_three, bins=self.mesh, range=bounds)
                self.i_hist_three = i_hist_three.ravel() / total_I_num / volume

    def outputCube(self):
        output_start_time = time.time()
        base = re.match(r"(\d{3}.*?LiI).*?",
                        args.trjfile.split("/")[-1]).group(1)
        self.atomsDic = {'I': '53', 'Li': '3', 'P': '15', 'S': '16'}

        def get_header():
            body = f"created from {__file__}, {args}\n"
            body += "Contains the selected quantity on a FFT grid\n"
            origin = [-(self.cutoff)/ang_borr] * 3
            body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(5, *origin)
            body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(
                self.mesh[0], (self.cutoff*2)/self.mesh[0]/ang_borr, 0, 0)
            body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(
                self.mesh[1], 0, (self.cutoff*2)/self.mesh[1]/ang_borr, 0)
            body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(
                self.mesh[2], 0, 0, (self.cutoff*2)/self.mesh[2]/ang_borr)
            body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(
                int(self.atomsDic["P"]), float(self.atomsDic["P"]), *self.ps4_coord[0])
            for s_coord in self.ps4_coord[1:5]:
                body += "{:>5d}{:>12.7f}{:>12.7f}{:>12.7f}{:>12.7f}\n".format(
                    int(self.atomsDic["S"]), float(self.atomsDic["S"]), *s_coord)
            return body

        def save_cube(hist_data, suffix):
            if hist_data is None:
                return
            body = get_header()
            for idx, r in enumerate(hist_data):
                if idx % 6 == 5:
                    body += "{:>13.5E}\n".format(r)
                else:
                    body += "{:>13.5E}".format(r)
            if idx % 6 != 5:
                body += "\n"  # 最後の行で改行がない場合用

            outfile = f"{dirname}/{base}_PS4_{suffix}.cube"
            with open(outfile, "w") as o:
                o.write(body)
            print(f"{outfile} was created.")

        save_cube(self.li_hist, "Li")
        save_cube(self.i_hist, "I_All")
        save_cube(self.i_hist_zero, "I_zero")
        save_cube(self.i_hist_corner, "I_corner")
        save_cube(self.i_hist_edge, "I_edge")
        save_cube(self.i_hist_three, "I_three")

        output_end_time = time.time()
        print(f"output_time : {output_end_time - output_start_time:.2f} s")


if __name__ == "__main__":
    trj = LammpsTrj()
    trj.makeCube(args.trjfile)
    trj.outputCube()
