import numpy as np
import os, sys

from PIL import Image  # CC="cc -mavx2" pip install -U --force-reinstall pillow-simd
from torchvision import transforms
from torch.utils.data import Dataset
import cv2  # For testing code only
from sklearn.neighbors import NearestNeighbors

#
# from ipdb import set_trace
"""
StaticDataset: For stable testing result, fixed random seed
A,A2 Only , No patch and multi patch
"""

class StaticDataset(Dataset):
    def __init__(self, sampling_config, data_config):
        # Access all parameters directly
        self.__dict__.update(sampling_config)
        self.__dict__.update(data_config)
        transform_list = []
        transform_list.extend([transforms.ToTensor(),
                               transforms.Normalize(mean=[0.485, 0.456, 0.406],
                                                    std=[0.229, 0.224, 0.225])])
        self.transform = transforms.Compose(transform_list)
        self.cam_mat_inv = np.linalg.inv(self.cam_mat)
        # TODO: use self.object_classes for the full remap dict, but all sampling only A,A2 (match class only)
        self.clf_remap_dict = {0: 0}
        self.clf_remap_dict.update({k: i + 1 for i, k in enumerate(self.object_classes)})
        # Match class and non-match classes are fixed in StaticDataset, check all folder here
        assert len(self.match_classes) == 1, 'Static Dataset only run in single object scenes'
        self.match_class = self.match_classes[0]  # Single match class
        match_ids = self.class_to_object_ids[self.match_class]  # Class to object idss
        assert len(match_ids) == 1, 'Static Dataset only run in single class instance'
        self.match_id = match_ids[0]
        # Calculate the total len
        self.folders = self.id_to_scenes[self.match_id]

        self.sample_len = 0
        folder_cnt = []
        for fd_name, n_view in self.folders:
            self.sample_len += n_view - self.match_frame_near  #
            folder_cnt.append(self.sample_len)
        self.folder_cnt = np.asarray(folder_cnt)
        self.rs = np.random.RandomState(1234)

    def __getitem__(self, idx):  # Sampling Dataset
        # Sequentially
        """
        :param idx: Useless
        :return:
        Matching Class Set: K classes
        Non-Matching Class set: M classes
        img_A: containing Na objects from K
        img_A2: containing Na objects from K
        img_B: containing Nb objects from M

        Unique indices for each pixel points

        Sampling Strategy
        Sample K,M classes (Mutually Exclusive) -> Sample one scene for each class
        -> Sample 2 images from each scene for img_A and img_A2 (total n(K) scenes)
        -> Sample 1 image from each scene for img_B (total n(M) scenes)

        # All indices is row-majored reshaped image indices

        #>>> Part1 Total Data Points <<<
        mask_A_1d: indices of img_A that contain object A
        bg_A_1d: background indices in img_A (not contain any other objects)

        mask_A2_1d: all indices of img_A2 that contain object A
        bg_A2_1d: background indices in img_A2 (not contain any other objects)

        mask_B_1d: all indices of img_B that contain object B
        bg_B_1d: background indices in img_B (not contain any other objects)

        match_idx_A:
        match_idx_A2: All img_A,img_A2 matched indices (pair-order indices)
        # Note: Shuffle the paired indices to sample same object but non-matched pairs

        no_match_idx_A
        no_match_idx_A2: All no-match found indices (non-ordered indices, but not the background)

        # Note: Union(non_match_idx_A,match_idx_A) = mask_idx_A
        """
        # Sample scene A and object A(A2) id
        np.seterr(divide='ignore', invalid='ignore')

        while True:
            chk_match = 0
            while chk_match < 1:
                """
                match/non-match class set -> match/non-match object ids -> sample folders for each object
                -> sample match paired images ->extract foreground patches
                -> merge patches and match indices
                """
                class_mapping_dict = {self.match_id: self.match_class}
                folder_idx = np.where(self.folder_cnt > idx)[0][0]
                folder_name = self.folders[folder_idx][0]  # ge the name from [[Name,N]]
                if folder_idx > 0:
                    p_start = idx - self.folder_cnt[folder_idx - 1]
                else:
                    p_start = idx
                p_end = p_start + self.rs.randint(1, self.match_frame_near + 1)  # TODO: Fixed seed randomization here
                # [img_m11,img_m12,img_m21,img_m22,...], 2D image
                match_ids = [self.match_id]
                match_folder_ids = [(folder_name, [p_start, p_end])]
                match_img = [self.open_img(folder, 'color', p, 0)
                             for folder, pair in match_folder_ids for p in pair]
                img_A, img_A2 = match_img
                # [[mask2d11,mask2d12],[mask2d21,mask2d22],...], 2d mask
                match_mask = [[self.open_img(folder, 'label', pair[0], 1),
                               self.open_img(folder, 'label', pair[1], 1)] \
                              for folder, pair in match_folder_ids]

                # [mask11,mask12,mask21,mask22,...], 1d mask
                match_mask_1d = [np.where(mask2d.reshape(-1) == obj_id)[0] for masks, obj_id in
                                 zip(match_mask, [self.match_id]) for mask2d in masks]
                match_bg_1d = [np.where(mask2d.reshape(-1) == 0)[0] for masks in match_mask for mask2d in masks]
                match_diag = get_box_diagonal_length(img_A2.shape, match_mask_1d[1])
                # [depth11,depth12,depth21,depth22,...], 2D depth
                match_depth = [self.open_img(folder, 'depth', p, 1)
                               for folder, pair in match_folder_ids for p in pair]
                match_transforms = [self.relative_transform(folder, pair[0], pair[1]) for folder, pair in
                                    match_folder_ids]

                # (src,tgt) for each matching pair

                match_src_indices = []  # For pair src
                match_tgt_indices = []  # For pair tgt
                for i in range(0, len(match_mask_1d), 2):  # 2 images per pair match
                    src_indices, tgt_indices = self.projection_match(match_depth[i], match_mask_1d[i], \
                                                                     match_depth[i + 1], match_mask_1d[i + 1], \
                                                                     match_transforms[i // 2],
                                                                     self.dual_projection_check)
                    match_src_indices.append(src_indices)
                    match_tgt_indices.append(tgt_indices)

                chk_match = np.min([s.shape[0] for s in match_src_indices])
                if chk_match == 0:
                    print('One of the match is 0 after projection, resample')
                    continue
                # Keep only patch information (A little bit of bk, mostly object)
                # for i in range(len(match_img)):
                #     cv2.imshow('Test', match_img[i])
                #     while True:
                #         if cv2.waitKey(10) == ord('s'):
                #             break

                # BGR -> RGB for images (CV2)

            # Match result for sampling
            # All id is object id
            match_idx_A = np.hstack(match_src_indices)
            match_idx_A2 = np.hstack(match_tgt_indices)
            match_class_id = np.repeat(match_ids, [len(m) for m in match_src_indices])
            mask_A_1d = match_mask_1d[0]
            mask_A2_1d = match_mask_1d[1]

            fg_A_cls, fg_A2_cls = np.repeat(self.match_class, len(mask_A_1d)), np.repeat(self.match_class,
                                                                                len(mask_A2_1d))

            bg_A_1d = match_bg_1d[0]
            bg_A2_1d = match_bg_1d[1]

            """
            >>> Part2 Sample Data Points <<<
            Positive, Negative....
            """
            # For match -> As much as possible, not balance
            if self.balanced_match:
                s_match_idx_A, s_match_idx_A2 = self.sample_balanced_pairs(match_class_id, match_idx_A, match_idx_A2,
                                                                           self.match_n)
                s_match_hn_A, s_match_hn_A2 = self.sample_balanced_pairs(match_class_id, match_idx_A, match_idx_A2,
                                                                         self.hard_negative_match)
            else:
                s_match_idx_A, s_match_idx_A2 = self.sample_match_pairs(match_idx_A, match_idx_A2, N=self.match_n)
                s_match_hn_A, s_match_hn_A2 = self.sample_match_pairs(match_idx_A, match_idx_A2,
                                                                      N=self.hard_negative_match)
            # Non_match(_contrastive_closest_non_matchAA2)
            # non_match_idx_A, non_match_idx_A2 = self.calculate_non_match_expand(match_idx_A, match_idx_A2, mask_A_1d,
            #                                                                     mask_A2_1d, img_A.shape[0:2],
            #                                                                     self.non_match_kernel)

            # For Foreground <-> Background M (_objects_background)
            s_fg_A = self.sample_balanced_elements(fg_A_cls, mask_A_1d, self.background_n_per_class)
            s_bg_A = self.sample_elements_1d(bg_A_1d, len(s_fg_A))

            s_fg_A2 = self.sample_balanced_elements(fg_A2_cls, mask_A2_1d, self.background_n_per_class)
            s_bg_A2 = self.sample_elements_1d(bg_A2_1d, len(s_fg_A2))

            # Background Same (_background_same)
            s_bg_A_same = self.sample_elements_1d(bg_A_1d, self.background_same_n)
            s_bg_A2_same = self.sample_elements_1d(bg_A2_1d, self.background_same_n)
            # s_bg_B_same = self.sample_elements_1d(bg_B_1d, self.background_same_n)

            # For closest pixel evaluation(_match_closest_pixel_distance)
            s_match_pix_A, s_match_pix_A2 = self.sample_balanced_pairs(match_class_id, match_idx_A, match_idx_A2,
                                                                       self.pix_dist_match_n)
            # For classification(mask)
            s_cls_label_A, s_cls_A = self.sample_balanced_elements(fg_A_cls, mask_A_1d, self.segmentation_n // 2,
                                                                   return_label=True)
            s_cls_label_A2, s_cls_A2 = self.sample_balanced_elements(fg_A2_cls, mask_A2_1d, self.segmentation_n // 2,
                                                                     return_label=True)
            s_cls_bk_A = self.sample_elements_1d(bg_A_1d, self.segmentation_n)

            # s_cls_labels = np.hstack((s_cls_label_A, s_cls_label_A2, np.zeros_like(s_cls_bk_A), s_cls_label_B))
            # Mapping class label from class_id to 0~N_classes that actually used in the sampling config, for segmentation prediction
            # if self.A_A2_only:
            s_cls_labels = np.hstack((s_cls_label_A, s_cls_label_A2, np.zeros_like(s_cls_bk_A)))
            s_cls_labels = self.remap_label_lists([s_cls_labels], self.clf_remap_dict)[0]

            # Sample mask for cross non_match (_contrastive_closest_non_matchAB)
            hn_sample_n = np.min([len(mask_A_1d), len(mask_A2_1d), self.hard_negative_AB_n])
            s_mask_A_hn = self.sample_elements_1d(mask_A_1d, hn_sample_n)
            s_mask_A2_hn = self.sample_elements_1d(mask_A2_1d, hn_sample_n)
            # Sample mask for _contrastive_closest_non_matchAA2, too big mask will use too many memory
            s_mask_A_hn_AA2 = self.sample_elements_1d(mask_A_1d, self.mask_max_hard_negative)
            s_mask_A2_hn_AA2 = self.sample_elements_1d(mask_A2_1d, self.mask_max_hard_negative)

            # BGR -> RGB
            img_A = img_A[:, :, [2, 1, 0]]
            img_A2 = img_A2[:, :, [2, 1, 0]]
            # img_B = img_B[:, :, [2, 1, 0]]
            # Transform
            img_A, img_A2 = tuple(map(self.transform, [img_A, img_A2]))
            match_class_set = np.array([self.match_class])
            empty_lst = np.array([-1])
            return_vals = (img_A, img_A2, np.zeros_like(img_A),
                           mask_A_1d, mask_A2_1d, empty_lst, \
                           fg_A_cls, fg_A2_cls, empty_lst, \
                           bg_A_1d, bg_A2_1d, empty_lst, \
                           match_class_set, empty_lst, \
                           s_match_idx_A, s_match_idx_A2, s_match_hn_A, s_match_hn_A2, \
                           s_mask_A_hn_AA2, s_mask_A2_hn_AA2, \
                           s_fg_A, s_bg_A, s_fg_A2, s_bg_A2, empty_lst, empty_lst, \
                           s_bg_A_same, s_bg_A2_same, empty_lst, \
                           s_match_pix_A, s_match_pix_A2, \
                           s_mask_A_hn, s_mask_A2_hn, \
                           empty_lst,empty_lst,empty_lst,\
                           s_cls_A, s_cls_A2, empty_lst, s_cls_bk_A, s_cls_labels, match_diag)
            if self.return_checker(return_vals):
                break
            else:
                print('Dataset Re-sample, Return Checker Fails')
        return return_vals

    def remap_label_lists(self, id_list, mapping):
        """
        :param id_list: list of 1d ndarray
        :param mapping: dictionary of m
        :return:
        """
        return [replace_with_dict(l, mapping) for l in id_list]

    def sample_balanced_elements(self, class_label, elements, N_per_class, return_label=False):
        """
        :param class_label: integer class label in ndarray
        :param elements: list of items for sampling
        :param N: N for each class for sampling
        :return: s_class_label,s_elements
        """
        labels, counts = np.unique(class_label, return_counts=True)
        sample_n = min(np.min(counts), N_per_class)
        s_elements = np.hstack([self.rs.choice(elements[class_label == l], sample_n) for l in labels])
        if return_label:
            return np.repeat(labels, sample_n), s_elements
        else:
            return s_elements

    def sample_balanced_pairs(self, class_label, elements_1, elements_2, max_N, return_label=False):
        """
        :param class_label: integer class label in ndarray ,for pair elements_1,elements_2
        :param elements_1: list of items for sampling
        :param elements_2: list of items for sampling
        :param N: N for each class for sampling
        :return: s_class_label,s_elements
        """
        labels, counts = np.unique(class_label, return_counts=True)
        sample_n = min(np.min(counts), max_N // len(labels))
        s_1 = []
        s_2 = []
        for l in labels:
            select = self.rs.choice(np.where(class_label == l)[0], sample_n)
            s_1.append(elements_1[select])
            s_2.append(elements_2[select])
        if return_label:
            return np.repeat(labels, sample_n), np.hstack(s_1), np.hstack(s_2)
        else:
            return np.hstack(s_1), np.hstack(s_2)

    def sample_elements_1d(self, elements, N):
        N_sample = min(len(elements), N)
        idx = random_permute_derangement(len(elements), self.rs)
        return elements[idx[0:N_sample]]

    def projection_match(self, src_depth_img, src_obj_mask, tgt_depth_img, tgt_obj_mask, tgtTsrc, dual_check=False):
        """
        :param src_depth_img: 2D depth image
        :param src_obj_mask: 1D pixel indices that contains the specific object in src image
        :param tgt_depth_img: 2D depth image
        :param tgt_obj_mask: 1D pixel indices that contains the specific object in target image
        :param tgtTsrc: tgtTsrc: 4*4 transform matrix tgt<-src
        :param dual_check: Dual Projection check for matching
        :return: two 1D pixel index array for src frame and target frame, paired order
        """
        width, height = src_depth_img.shape[1], src_depth_img.shape[0]
        u_src = np.tile(np.arange(0, width), height)
        v_src = np.repeat(np.arange(0, height), width)
        z_src = src_depth_img.reshape(-1) * self.depth_scale

        uz_src, yz_src = np.multiply(u_src, z_src), np.multiply(v_src, z_src)
        # TODO: Apply src object mask, then calculate source x,y,z (may slightly faster?)
        uvz_src = np.vstack((uz_src, yz_src, z_src))  # [src_obj_mask]
        xyz_src = np.dot(self.cam_mat_inv, uvz_src)  # 3,len(src_obj_mask)
        xyz_src_homo = np.vstack((xyz_src, np.ones(xyz_src.shape[1])))
        # Project to target frame
        xyz_tgt = np.dot(tgtTsrc, xyz_src_homo)[0:3, :]
        uvz_tgt = np.dot(self.cam_mat, xyz_tgt)
        u_tgt = np.clip(np.round(np.divide(uvz_tgt[0], uvz_tgt[2])).astype(np.int32), a_min=0, a_max=width - 1)
        v_tgt = np.clip(np.round(np.divide(uvz_tgt[1], uvz_tgt[2])).astype(np.int32), a_min=0, a_max=height - 1)
        # For nan case
        z_tgt_projected = uvz_tgt[2]  # From source frame and pose
        z_tgt_depth = tgt_depth_img[v_tgt, u_tgt]  # From depth measurement, query projected points, # 640*480 values

        # Compared calculated z and true depth value
        match_depth_indices = \
            np.where(np.abs(z_tgt_depth * self.depth_scale - z_tgt_projected) < self.point_distance_check)[0]

        # Construct match indices in 1-D

        # pixel indices for target image
        match_idx_tgt = v_tgt[match_depth_indices] * width + u_tgt[match_depth_indices]
        # pixel indices for source image
        match_idx_src = match_depth_indices

        # Filter-out background and other object
        match_idx_tgt, keep_indices_tgt, _ = np.intersect1d(match_idx_tgt, tgt_obj_mask, return_indices=True)
        match_idx_src, keep_indices_src, _ = np.intersect1d(match_idx_src[keep_indices_tgt], src_obj_mask,
                                                            return_indices=True)
        match_idx_tgt = match_idx_tgt[keep_indices_src]  # Foreground object in both sides
        assert match_idx_tgt.shape[0] == match_idx_src.shape[0]
        # match_idx_src, match_idx_tgt

        # print('Match Points', match_idx_tgt.shape[0])
        if dual_check:
            # match_idx_A_AA2, match_idx_A2_AA2 = self.projection_match(depth_A, mask_A_1d, depth_A2, mask_A2_1d,
            #                                                           A2_T_A)
            match_idx_tgt2, match_idx_src2 = self.projection_match(tgt_depth_img, tgt_obj_mask, src_depth_img,
                                                                   src_obj_mask, np.linalg.inv(tgtTsrc),
                                                                   dual_check=False)
            match_idx_src, keep_src_idx, _ = np.intersect1d(match_idx_src, match_idx_src2, return_indices=True)
            match_idx_tgt, keep_tgt_idx, _ = np.intersect1d(match_idx_tgt[keep_src_idx], match_idx_tgt2,
                                                            return_indices=True)
            match_idx_src = match_idx_src[keep_tgt_idx]

        return match_idx_src, match_idx_tgt  # 1D index in image

    def sample_match_pairs(self, match_idx_A, match_idx_A2, N):  # Match
        assert len(match_idx_A) == len(match_idx_A2)
        N_sample = min(len(match_idx_A), N)
        rand_idx = self.rs.permutation(len(match_idx_A))[0:N_sample]
        return match_idx_A[rand_idx], match_idx_A2[rand_idx]

    def open_img(self, folder, info_type, idx, mode):
        if mode == 0:  # RGB
            return cv2.imread(os.path.join(self.base_dir, folder, '{:06}-{}.png'.format(idx, info_type)))
        else:  # Mask,Depth
            return cv2.imread(os.path.join(self.base_dir, folder, '{:06}-{}.png'.format(idx, info_type)),
                              cv2.IMREAD_ANYDEPTH)

    def relative_transform(self, folder, idx1, idx2):
        # Retuen the transformation of idx2_T_idx1
        pose_1 = np.loadtxt(
            os.path.join(self.base_dir, folder, '{:06}-pose.txt'.format(idx1)))
        pose_2 = np.loadtxt(
            os.path.join(self.base_dir, folder, '{:06}-pose.txt'.format(idx2)))
        return np.dot(np.linalg.inv(pose_2), pose_1)

    def __len__(self):
        return self.sample_len

    @staticmethod
    def return_checker(return_tuple):
        for item in return_tuple:
            if isinstance(item, np.ndarray):
                if item.shape[0] == 0:
                    return False
            if isinstance(item, list):
                for k in item:
                    if isinstance(item, np.ndarray):
                        if item.shape[0] == 0:
                            return False
        return True

def cv2_show_img_lists(img_list, msg=''):
    print(msg)
    print('s for next, q for exit')
    for i in range(len(img_list)):
        cv2.imshow('Test', img_list[i])
        while True:
            if cv2.waitKey(10) == ord('s'):
                break

# https://stackoverflow.com/questions/47171356/replace-values-in-numpy-array-based-on-dictionary-and-avoid-overlap-between-new
def replace_with_dict(ar, dic):
    # Extract out keys and values
    k = np.array(list(dic.keys()))
    v = np.array(list(dic.values()))

    # Get argsort indices
    sidx = k.argsort()

    ks = k[sidx]
    vs = v[sidx]
    return vs[np.searchsorted(ks, ar)]

# Guarantee no element stays in the same position after random permutation, for indexing only
def random_permute_derangement(N, np_rs):
    original = np.arange(N)
    new = np_rs.permutation(N)
    same = np.where(original == new)[0]
    if N == 1:
        # Warning("Invalid Derangement")
        return [0]
    while len(same) != 0:
        swap = same[np_rs.permutation(len(same))]
        new[same] = new[swap]
        same = np.where(original == new)[0]
        if len(same) == 1:
            swap = np_rs.randint(0, N)
            new[[same[0], swap]] = new[[swap, same[0]]]
    return new

def get_box_diagonal_length(img_shape, mask_1d):
    h, w, _ = img_shape
    u, v = mask_1d % w, mask_1d // w
    u1, u2 = np.min(u), np.max(u)
    v1, v2 = np.min(v), np.max(v)
    diag = np.sqrt((u1 - u2) ** 2 + (v1 - v2) ** 2)
    return diag

def static_dataset_vis_test():
    disp_max = 5000
    os.putenv('DISPLAY', ':0')
    import yaml
    with open('YOUR_CONFIG.yaml',
              'r') as f:
        dd = yaml.load(f)
    pd = StaticDataset(dd['sampling'], dd['test_data'])
    pd.transform = lambda x: x  # No normalization, for visualization
    pd.match_n = disp_max
    pd.background_n = 50000
    keys = ['mask_A_1d', 'mask_A2_1d', 'mask_B_1d', \
            'fg_A_cls', 'fg_A2_cls', 'fg_B_cls', \
            'match_class_set', 'non_match_class_set', \
            's_match_idx_A', 's_match_idx_A2', 's_match_hn_A', 's_match_hn_A2', \
            's_mask_A_hn_AA2', 's_mask_A2_hn_AA2',
            's_fg_A', 's_bg_A', 's_fg_A2', 's_bg_A2', 's_fg_B', 's_bg_B', \
            's_bg_A_same', 's_bg_A2_same', 's_bg_B_same', \
            's_match_pix_A', 's_match_pix_A2', \
            's_mask_A_hn', 's_mask_A2_hn', \
            's_cls_A', 's_cls_A2', 's_cls_B', 's_cls_bk_A', 's_cls_labels']
    test_pairs = [[0, 1, False], [8, 9, True], [10, 11, True], [12, 13, False],
                  [14, 16, False], [15, 17, False],
                  [20, 21, False], [23, 24, True],
                  [25, 26, False], [27, 28, False]]

    while True:
        values = pd[0]
        # set_trace()
        img_A, img_A2, img_B, match_idx_src, match_idx_tgt = \
            values[0], values[1], values[2], values[11], values[12]  # A AF
        data = values[3:]

        print('Match N=', match_idx_src.shape[0])
        img_concate3 = np.concatenate((img_A, img_A2), axis=1)
        img_concate3 = np.ascontiguousarray(img_concate3[:, :, [2, 1, 0]])
        img_concate2 = np.concatenate((img_A, img_A2), axis=1)
        img_concate2 = np.ascontiguousarray(img_concate2[:, :, [2, 1, 0]])
        cv2.imshow("Test", img_concate3)
        while True:
            if cv2.waitKey(10) == ord('s'):
                break
        for ti, tp in enumerate(test_pairs):
            # if ti == 2:
            #     break
            if len(tp) == 3:  # A A2
                disp_img = np.array(img_concate2)
                src_u, src_v = indices1d_remap_new_uv(data[tp[0]], img_A.shape, (480, 640, 3))
                tgt_u, tgt_v = indices1d_remap_new_uv(data[tp[1]], img_A2.shape, (480, 640, 3), shift=(640, 0))
                sample_n = min(len(src_u), len(tgt_u))
                step = max(1, int(sample_n // disp_max))
                if tp[2]:
                    for i in range(0, sample_n, step):
                        pts_src = src_u[i], src_v[i]
                        pts_tgt = tgt_u[i], tgt_v[i]
                        colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                        colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                        cv2.circle(disp_img, pts_src, 2, colors, thickness=-1)  # Filled circle
                        cv2.circle(disp_img, pts_tgt, 2, colors, thickness=-1)  # Filled circle
                        cv2.line(disp_img, pts_src, pts_tgt, colors)
                else:
                    for i in range(0, len(src_u), step):
                        pts_src = src_u[i], src_v[i]
                        colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                        colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                        cv2.circle(disp_img, pts_src, 2, colors, thickness=-1)  # Filled circle
                    for i in range(0, len(tgt_u), step):
                        pts_tgt = tgt_u[i], tgt_v[i]
                        colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                        colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                        cv2.circle(disp_img, pts_tgt, 2, colors, thickness=-1)  # Filled circle

                print(keys[tp[0]], ' & ', keys[tp[1]])
            else:  # A,A2,B
                disp_img = np.array(img_concate3)
                src_u, src_v = indices1d_remap_new_uv(data[tp[0]], img_A.shape, (480, 640, 3))
                tgt_u, tgt_v = indices1d_remap_new_uv(data[tp[1]], img_A2.shape, (480, 640, 3), shift=(640, 0))
                bu, bv = indices1d_remap_new_uv(data[tp[2]], img_B.shape, (480, 640, 3), shift=(1280, 0))
                sample_n = np.min([len(src_u), len(tgt_u), len(bu)])
                step = max(1, int(sample_n // disp_max))
                for i in range(0, len(src_u), step):
                    pts_src = src_u[i], src_v[i]
                    colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                    colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                    cv2.circle(disp_img, pts_src, 2, colors, thickness=-1)  # Filled circle
                for i in range(0, len(tgt_u), step):
                    pts_src = tgt_u[i], tgt_v[i]
                    colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                    colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                    cv2.circle(disp_img, pts_src, 2, colors, thickness=-1)  # Filled circle
                for i in range(0, len(bu), step):
                    pts_src = bu[i], bv[i]
                    colors = (np.random.randint(0, 256), np.random.randint(0, 256), np.random.randint(0, 256))
                    colors = (int(colors[0]), int(colors[1]), int(colors[2]))
                    cv2.circle(disp_img, pts_src, 2, colors, thickness=-1)  # Filled circle

                print(keys[tp[0]], ' & ', keys[tp[1]], ' & ', keys[tp[2]])
            cv2.imshow("Test", disp_img)
            while True:
                if cv2.waitKey(10) == ord('s'):
                    break
                if cv2.waitKey(10) == ord('q'):
                    return
        # bg_A = values[-22]
        # print('BG Size', len(bg_A))
        # for i in range(0, len(bg_A), 10):
        #     idx_src = bg_A[i]
        #     pts_src = (idx_src % 640, int(idx_src / 640))
        #     # pts_tgt = (idx_tgt % 640 + 640, int(idx_tgt / 640))
        #     # rand_color = tuple(np.random.randint(0, 255, 3).astype(int))
        #     colors = (0, 0, 0)
        #     colors = (int(colors[0]), int(colors[1]), int(colors[2]))
        #     # cv2.line(img_concate, pts_src, pts_tgt, colors)q
        #     cv2.circle(img_concate, pts_src, 2, colors, thickness=-1)  # Filled circle
        #     # cv2.circle(img_concate, pts_tgt, 2, colors, thickness=-1)  # Filled circle

# function for visualize testing
def indices1d_remap_new_uv(old_indices, old_shape, new_shape, shift=(0, 0)):  # Assume patch placed on left-upper
    """
    :param old_indices:  1d index list for old_shape
    :param old_shape:  h,w of new image
    :param new_shape: h,w of new image
    :param shift: Horizontal and vertical shift from (left-upper) origin (shift_hori,shift_vert)
    :return:
    """
    h0, w0, _ = old_shape
    h1, w1, _ = new_shape
    shift_w, shift_h = shift
    # Old to 2d ->
    idx_h, idx_w = old_indices // w0, old_indices % w0
    u, v = idx_w + shift_w, idx_h + shift_h
    return u, v

if __name__ == '__main__':
    static_dataset_vis_test()


import pandas as pd
import numpy as np
import pickle
import cv2
import matplotlib.pyplot as plt

pickle_in = open('emotion_detection_model.p','rb')
model = pickle.load(pickle_in)

def pred(img):
    img = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    img = cv2.resize(img,(48,48))
    pred = model.predict(img[np.newaxis,:,:,np.newaxis])
    return str(np.argmax(pred.astype('int')))

# Making Predictions on Real-Time Video

cap = cv2.VideoCapture(0)

while 1:
    ret,frame = cap.read()
    frame = cv2.flip(frame,1)
    print(pred(frame))
    cv2.putText(frame,pred(frame),(50,50),cv2.FONT_HERSHEY_DUPLEX,1,(0,255,0))
    cv2.imshow('frame',frame)

    if cv2.waitKey(1)& 0xFF==ord('q'):
        break


import os
import cv2
import copy
import torch
import torch.nn as nn
import torch.nn.functional as F
import numpy as np

from .backbones.vit_pytorch import vit_base_patch16_224_TransReID
# from loss.metric_learning import Arcface, Cosface, AMSoftmax, CircleLoss, ContrastiveLoss
from loss.triplet_loss import TripletLoss
# pose-net
from model.pose_net import SimpleHRNet

class GELU(nn.Module):
    def __init__(self):
        super(GELU, self).__init__()
    def forward(self, x):
        return 0.5*x*(1+F.tanh(np.sqrt(2/np.pi)*(x+0.044715*torch.pow(x,3))))

# From PyTorch internals
def _ntuple(n):
    def parse(x):
        if isinstance(x, container_abcs.Iterable):
            return x
        return tuple(repeat(x, n))
    return parse

def weights_init_kaiming(m):
    classname = m.__class__.__name__
    if classname.find('Linear') != -1:
        nn.init.kaiming_normal_(m.weight, a=0, mode='fan_out')
        nn.init.constant_(m.bias, 0.0)

    elif classname.find('Conv') != -1:
        nn.init.kaiming_normal_(m.weight, a=0, mode='fan_in')
        if m.bias is not None:
            nn.init.constant_(m.bias, 0.0)
    elif classname.find('BatchNorm') != -1:
        if m.affine:
            nn.init.constant_(m.weight, 1.0)
            nn.init.constant_(m.bias, 0.0)

def weights_init_classifier(m):
    classname = m.__class__.__name__
    if classname.find('Linear') != -1:
        nn.init.normal_(m.weight, std=0.001)
        if m.bias:
            nn.init.constant_(m.bias, 0.0)

def _get_activation_fn(activation):
    """Return an activation function given a string"""
    if activation == "relu":
        return F.relu
    if activation == "gelu":
        return F.gelu
    if activation == "glu":
        return F.glu
    raise RuntimeError(F"activation should be relu/gelu, not {activation}.")

class TransformerDecoderLayer(nn.Module):

    def __init__(self, d_model, nhead, dim_feedforward=2048, dropout=0.1,
                 activation="relu", normalize_before=False):
        super().__init__()
        self.self_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout)
        self.multihead_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout)
        # Implementation of Feedforward model
        self.linear1 = nn.Linear(d_model, dim_feedforward)
        self.dropout = nn.Dropout(dropout)
        self.linear2 = nn.Linear(dim_feedforward, d_model)

        self.norm1 = nn.LayerNorm(d_model)
        self.norm1 = self.norm1.requires_grad_()
        self.norm2 = nn.LayerNorm(d_model)
        # self.norm2 = self.norm2.requires_grad_()
        self.norm3 = nn.LayerNorm(d_model)
        self.dropout1 = nn.Dropout(dropout)
        self.dropout2 = nn.Dropout(dropout)
        self.dropout3 = nn.Dropout(dropout)

        self.activation = _get_activation_fn(activation)
        self.normalize_before = normalize_before

    def with_pos_embed(self, tensor, pos):
        return tensor if pos is None else tensor + pos

    def forward_post(self, prototype, global_feat,
                     pos, query_pos):
        q = k = self.with_pos_embed(prototype, query_pos)
        prototype_2 = self.self_attn(q, k, value=prototype)[0]
        prototype = prototype + self.dropout1(prototype_2)
        prototype = self.norm1(prototype)
        out_prototype = self.multihead_attn(query=self.with_pos_embed(prototype, query_pos),
                                   key=self.with_pos_embed(global_feat, pos),
                                   value=global_feat)[0]
        prototype = prototype + self.dropout2(out_prototype)
        prototype = self.norm2(prototype)
        prototype = self.linear2(self.dropout(self.activation(self.linear1(prototype))))
        prototype = prototype + self.dropout3(prototype)
        prototype = self.norm3(prototype)
        return prototype

    def forward(self, prototype, global_feat, pos=None, query_pos=None):
        return self.forward_post(prototype, global_feat, pos, query_pos)

class Mlp(nn.Module):
    def __init__(self, in_features, hidden_features=None, out_features=None, act_layer=nn.GELU, drop=0.): #nn.GELU
        super().__init__()
        out_features = out_features or in_features
        hidden_features = hidden_features or in_features
        self.fc1 = nn.Linear(in_features, hidden_features)
        self.act = act_layer()
        self.fc2 = nn.Linear(hidden_features, out_features)
        self.drop = nn.Dropout(drop)

    def forward(self, x):
        x = self.fc1(x)
        x = self.act(x)
        x = self.drop(x)
        x = self.fc2(x)
        x = self.drop(x)
        return x

def _get_clones(module, N):
    return nn.ModuleList([copy.deepcopy(module) for i in range(N)])

class TransformerDecoder(nn.Module):

    def __init__(self, decoder_layer, num_layers, norm=None, return_intermediate=False):
        super().__init__()
        self.layers = _get_clones(decoder_layer, num_layers)
        self.num_layers = num_layers
        self.norm = norm
        self.return_intermediate = return_intermediate

    def forward(self, prototype, global_feat,
                pos = None, query_pos = None):
        output = prototype
        intermediate = []
        for layer in self.layers:
            output = layer(output, global_feat,
                           pos=pos, query_pos=query_pos)
            if self.return_intermediate:
                intermediate.append(self.norm(output))
        if self.norm is not None:
            output = self.norm(output)
            if self.return_intermediate:
                intermediate.pop()
                intermediate.append(output)
        if self.return_intermediate:
            return torch.stack(intermediate)
        return output

class build_skeleton_transformer(nn.Module):

    def __init__(self, num_classes, camera_num, view_num, cfg, factory):
        super(build_skeleton_transformer, self).__init__()
        model_path = cfg.MODEL.PRETRAIN_PATH
        pretrain_choice = cfg.MODEL.PRETRAIN_CHOICE
        self.in_planes = 768
        self.pose_dim = 2048

        # pose config
        self.pose = SimpleHRNet(48,
                                17,
                                cfg.MODEL.POSE_WEIGHT,
                                model_name='HRNet',
                                resolution=(256, 128),
                                interpolation=cv2.INTER_CUBIC,
                                multiperson=False,
                                return_heatmaps=True,
                                return_bounding_boxes=False,
                                max_batch_size=32,
                                device=torch.device("cuda")
                                )

        # self.alphapose = SingleImageAlphaPose(alphapose_args, alphapose_cfg, device=torch.device("cuda"))

        self.skeleton_threshold = cfg.MODEL.SKT_THRES

        # decoderlayer config
        self.num_head = cfg.MODEL.NUM_HEAD
        self.dim_forward = 2048
        self.decoder_drop = cfg.MODEL.DECODER_DROP_RATE
        self.drop_first = cfg.MODEL.DROP_FIRST

        # decoder config
        self.decoder_numlayer = cfg.MODEL.NUM_DECODER_LAYER
        self.decoder_norm = nn.LayerNorm(self.in_planes)

        # query setting
        self.num_query = cfg.MODEL.QUERY_NUM
        self.query_embed = nn.Embedding(cfg.MODEL.QUERY_NUM, self.in_planes).weight

        # part view based decoder
        self.transformerdecoderlayer = TransformerDecoderLayer(self.in_planes, self.num_head, self.dim_forward, self.decoder_drop, "relu", self.drop_first)
        self.transformerdecoder = TransformerDecoder(self.transformerdecoderlayer, self.decoder_numlayer, self.decoder_norm)

        print('using Transformer_type: {} as a encoder'.format(cfg.MODEL.TRANSFORMER_TYPE))

        # visual context encoder
        # Thanks the authors of TransReID https://github.com/heshuting555/TransReID.git
        self.base_vit = factory[cfg.MODEL.TRANSFORMER_TYPE](img_size=cfg.INPUT.SIZE_TRAIN, sie_xishu=cfg.MODEL.SIE_COE, local_feature=cfg.MODEL.JPM, camera=camera_num, view=view_num, stride_size=cfg.MODEL.STRIDE_SIZE, drop_path_rate=cfg.MODEL.DROP_PATH)

        print('Loading pretrained ImageNet model......from {}'.format(model_path))
        if pretrain_choice == 'imagenet':
            self.base_vit.load_param(model_path)
            print('Loading pretrained ImageNet model......from {}'.format(model_path))

        block = self.base_vit.blocks[-1]
        layer_norm = self.base_vit.norm
        self.b2 = nn.Sequential(
            copy.deepcopy(block),
            copy.deepcopy(layer_norm)
        )

        self.num_classes = num_classes
        self.bottleneck = nn.BatchNorm1d(self.in_planes)
        self.bottleneck.bias.requires_grad_(False)
        self.bottleneck.apply(weights_init_kaiming)
        self.bottleneck_decoder = nn.BatchNorm1d(self.in_planes)
        self.bottleneck_decoder.bias.requires_grad_(False)
        self.bottleneck_decoder.apply(weights_init_kaiming)
        self.non_skt_decoder = nn.BatchNorm1d(self.in_planes)
        self.non_skt_decoder.bias.requires_grad_(False)
        self.non_skt_decoder.apply(weights_init_kaiming)

        self.ID_LOSS_TYPE = cfg.MODEL.ID_LOSS_TYPE
        if self.ID_LOSS_TYPE == 'triplet':
            self.classifier = TripletLoss(margin=cfg.SOLVER.COSINE_MARGIN, hard_factor=cfg.SOLVER.HARD_FACTOR)
        else:
            self.classifier = nn.Linear(self.in_planes, self.num_classes, bias=False)

        for i in range(17):
            exec('self.classifier_{} = nn.Linear(self.in_planes, self.num_classes, bias=False)'.format(i+1))
            exec('self.classifier_{}.apply(weights_init_classifier)'.format(i+1))

        for i in range(17):
            exec('self.bottleneck_{} = nn.BatchNorm1d(self.in_planes)'.format(i+1))
            exec('self.bottleneck_{}.bias.requires_grad_(False)'.format(i+1))
            exec('self.bottleneck_{}.apply(weights_init_kaiming)'.format(i+1))

        self.classifier_encoder = nn.Linear(self.in_planes, self.num_classes, bias=False)
        self.classifier_encoder.apply(weights_init_classifier)
        self.classifier_decoder = nn.Linear(self.in_planes, self.num_classes, bias=False)
        self.classifier_decoder.apply(weights_init_classifier)
        self.pose_decoder_linear = nn.Linear(self.pose_dim , self.in_planes)
        self.pose_avg = nn.AdaptiveAvgPool2d((1, self.in_planes))
        self.non_parts = nn.AdaptiveAvgPool2d((1, self.in_planes))
        self.decoder_global = nn.AdaptiveAvgPool2d((1, self.in_planes))

        for i in range(self.num_query):
            exec('self.classifier_decoder_{} = nn.Linear(self.in_planes, self.num_classes, bias=False)'.format(i+1))
            exec('self.classifier_decoder_{}.apply(weights_init_classifier)'.format(i+1))

        for i in range(self.num_query):
            exec('self.bottleneck_decoder_{} = nn.BatchNorm1d(self.in_planes)'.format(i+1))
            exec('self.bottleneck_decoder_{}.bias.requires_grad_(False)'.format(i+1))
            exec('self.bottleneck_decoder_{}.apply(weights_init_kaiming)'.format(i+1))

    def forward(self, x, label=None, cam_label= None, view_label=None): #ht optinal

        bs, c, h, w = x.shape # [batch, 3, 256, 128]

        # HRNet:
        heatmaps, joints = self.pose.predict(x)
        heatmaps = torch.from_numpy(heatmaps).cuda()    #[bs, 17, 64, 32]

        heatmaps = heatmaps.view(bs, heatmaps.shape[1], -1) # [bs, 17, 2048]

        ttt = heatmaps.cpu().numpy()
        skt_ft = np.zeros((heatmaps.shape[0], heatmaps.shape[1]), dtype=np.float32)

        for i, heatmap in enumerate(ttt):  #[64]
            for j, joint in enumerate(heatmap): #[17]

                if max(joint) < self.skeleton_threshold:
                    skt_ft[i][j] = 1    # Eq 4 in paper

        skt_ft = torch.from_numpy(skt_ft).cuda()    #[64, 17]

        pose_align_wt = self.pose_decoder_linear(heatmaps)  #[bs, 17, 768] FC

        heat_wt = self.pose_avg(heatmaps) #[bs, 1, 768]

        features = self.base_vit(x, cam_label=cam_label, view_label=view_label) # [64, 129, 768] ViT

        # Input of decoder
        decoder_value = features * heat_wt
        decoder_value = decoder_value.permute(1,0,2)

        # strip
        feature_length = features.size(1) - 1   #128
        patch_length = feature_length // self.num_query  #128 // 17
        token = features[:, 0:1]
        x = features[:, 1:]

        sim_feat = []
        # Encoder group features
        for i in range(16):
            exec('b{}_local = x[:, patch_length*{}:patch_length*{}]'.format(i+1, i, i+1))

            exec('b{}_local_feat = self.b2(torch.cat((token, b{}_local), dim=1))'.format(i+1, i+1))
            # exec('print(b{}_local_feat.shape)'.format(i+1))
            exec('local_feat_{} = b{}_local_feat[:, 0]'.format(i+1, i+1))

            exec('sim_feat.append(local_feat_{})'.format(i+1))

        b17_local = x[:, patch_length*16:]
        b17_local_feat = self.b2(torch.cat((token, b17_local), dim=1))
        local_feat_17 = b17_local_feat[:, 0]
        sim_feat.append(local_feat_17)

        # inference list
        inf_encoder = []
        # BN
        for i in range(17):
            exec('local_feat_{}_bn = self.bottleneck_{}(local_feat_{})'.format(i+1, i+1, i+1))
            exec('inf_encoder.append(local_feat_{}_bn/17)'.format(i+1))

        feat = features[:, 0].unsqueeze(1) * heat_wt + features[:, 0].unsqueeze(1)

        feat = feat.squeeze(1)

        # f_gb feature from encoder
        global_out_feat = self.bottleneck(feat) #[bs, 768]

        # part views
        query_embed = self.query_embed

        query_embed = query_embed.unsqueeze(1).repeat(1, bs, 1)

        prototype = torch.zeros_like(query_embed)

        # part-view based decoder
        out = self.transformerdecoder(prototype, decoder_value, query_pos=query_embed)

        # part view features
        last_out = out.permute(1,0,2)   # [bs, num_query, 768]

        sim_decoder = torch.stack(sim_feat, dim=1)  #[bs, 17, 768]

        #  PFA
        sim_decoder = PFA(sim_decoder, pose_align_wt) #[bs 17 768]

        #  PVM
        decoder_feature, ind = PVM(sim_decoder, last_out) #[bs num_query 768]

        decoder_gb = self.decoder_global(decoder_feature).squeeze(1)   #[bs, 1, 768]

        # non skt parts
        out_non_parts = []
        # skt parts
        out_skt_parts = []

        decoder_skt_feature = []

        decoder_non_feature = []

        for i in range(bs):
            non_skt_feat_list = []
            per_skt_feat_list = []

            skt_feat = skt_ft[i]   #[17]
            # non_zero_skt = torch.nonzero(skt_feat).squeeze(1) #[num]

            skt_part = skt_feat.cpu().numpy()
            skt_ind = np.argwhere(skt_part==0).squeeze(1) #[17-num] numpy type

            for j in range(decoder_feature.shape[1]):

                # version 1 use original heatmap label
                # if skt_feat[skt_ind[i][j]] == 0:
                #     non_feat = decoder_feature[i, j, :]
                #     non_skt_feat_list.append(non_feat)

                if skt_feat[ind[i][j]] == 1: # version 2 use PVM label
                    non_feat = decoder_feature[i, j, :]
                    non_skt_feat_list.append(non_feat)

                else:
                    skt_based_feat = decoder_feature[i, j, :] #[768]
                    per_skt_feat_list.append(skt_based_feat)

            if len(non_skt_feat_list) == 0:
                zero_feature = torch.zeros_like(decoder_gb[i])
                non_skt_feat_list.append(zero_feature)         #TODO:
            non_skt_single = torch.stack(non_skt_feat_list, dim=0).unsqueeze(0)  #[1, len(nonzero), 768]、

            decoder_non_feature.append(non_skt_single)
            non_skt_single = self.non_parts(non_skt_single) #[1, 1, 768]
            out_non_parts.append(non_skt_single) # [[1,1,768], [1,1,768], ....] bs length

            if len(per_skt_feat_list) == 0:
                per_skt_feat_list.append(decoder_gb[i])         #TODO:
            skt_single = torch.stack(per_skt_feat_list, dim=0).unsqueeze(0)     #[1, x, 768]

            decoder_skt_feature.append(skt_single)
            skt_single = self.non_parts(skt_single) #[1, 1, 768]
            out_skt_parts.append(skt_single)    # [[1,1,768], [1,1,768], ....] bs length

        last_non_parts = torch.cat(out_non_parts, dim=0)    #[bs, 1, 768]

        last_skt_parts = torch.cat(out_skt_parts, dim=0)    #[bs, 1, 768]

        # output high-confidence keypoint features
        decoder_out = self.bottleneck_decoder(last_skt_parts[:, 0]) #[bs, 768]

        # output non-skt-parts
        non_skt_parts = self.non_skt_decoder(last_non_parts[:, 0])

        # TODO:use last out or decoder out ??
        out_score = self.classifier_decoder(decoder_out)

        # Only high-confidence guided features are used to compute loss
        decoder_list = []

        # pad zeros for high-confidence guided features to self.num_query
        for i in decoder_skt_feature:
            if i.shape[1] < self.num_query:
                pad = torch.zeros((1,self.num_query-i.shape[1], self.in_planes)).to(i.device)
                pad_feat = torch.cat([i, pad], dim=1)  #[1, num_query, 768]
                decoder_list.append(pad_feat)
            else:
                decoder_list.append(i)

        decoder_lt = torch.cat(decoder_list, dim=0) # [64, self.num_query, 768]

        decoder_feature = decoder_lt

        # decoder parts features
        decoder_feat = [decoder_out]
        decoder_inf = []
        for i in range(self.num_query):
            exec('b{}_deocder_local_feat = decoder_feature[:, {}]'.format(i+1, i))
            exec('decoder_feat.append(b{}_deocder_local_feat)'.format(i+1))
            exec('decoder_inf.append(b{}_deocder_local_feat/self.num_query)'.format(i+1))

        # decoder BN
        for i in range(self.num_query):
            exec('decoder_local_feat_{}_bn = self.bottleneck_decoder_{}(b{}_deocder_local_feat)'.format(i+1, i+1, i+1))

        encoder_feat = [global_out_feat] + sim_feat

        if self.training:
            # encoder parts
            cls_score = self.classifier_encoder(global_out_feat)

            encoder_score = [cls_score]

            for i in range(17):

                exec('cls_score_{} = self.classifier_{}(local_feat_{}_bn)'.format(i+1, i+1, i+1))
                exec('encoder_score.append(cls_score_{})'.format(i+1))

            decoder_score = [out_score]

            # decoder parts
            for i in range(self.num_query):

                exec('decoder_cls_score_{} = self.classifier_decoder_{}(decoder_local_feat_{}_bn)'.format(i+1, i+1, i+1))
                exec('decoder_score.append(decoder_cls_score_{})'.format(i+1))

            return encoder_score, encoder_feat ,decoder_score, decoder_feat, non_skt_parts

        else:
            # Inferece concat
            inf_feat = [global_out_feat] + inf_encoder + [decoder_out] + decoder_inf
            inf_features = torch.cat(inf_feat, dim=1)

            return inf_features

    def load_param(self, trained_path):
        param_dict = torch.load(trained_path)
        for i in param_dict:
            # if 'w1' in i.replace('module.', '') or 'w2' in i.replace('module.', ''):
            #     continue
            self.state_dict()[i.replace('module.', '')].copy_(param_dict[i])
        print('Loading pretrained model from {}'.format(trained_path))

    def load_param_finetune(self, model_path):
        param_dict = torch.load(model_path)
        for i in param_dict:
            self.state_dict()[i].copy_(param_dict[i])
        print('Loading pretrained model for finetuning from {}'.format(model_path))

def get_cos_similar_matrix(v1, v2):
    num = np.dot(v1, np.array(v2).T)  # 向量点乘
    denom = np.linalg.norm(v1, axis=1).reshape(-1, 1) * np.linalg.norm(v2, axis=1)  # 求模长的乘积
    res = num / denom
    res[np.isneginf(res)] = 0
    return 0.5 + 0.5 * res

def PVM(matrix, matrix1):
    '''
    @matrix shape [bs, 17, 768]
    @matrix1 shape [bs, x, 768]
    '''

    assert matrix.shape[0] == matrix1.shape[0], 'Wrong shape'
    assert matrix.shape[2] == matrix1.shape[2], 'Wrong dimension'

    batch_size = matrix.shape[0] #[bs, 17, 768]
    # skt_num = matrix.shape[1]
    final_sim = F.cosine_similarity(matrix.unsqueeze(2), matrix1.unsqueeze(1), dim=3) #[bs, 17, x]

    _, ind = torch.max(final_sim, dim=2)    # ind.shape [bs, x]

    sim_match = []
    for i in range(batch_size):
        org_mat = matrix[i] #[17, C]
        sim_mat = matrix1[i] #[x, C]
        shuffle_mat = []

        for j in range(ind.shape[1]):
            new = org_mat[ind[i][j]] + sim_mat[j]  #[C]
            new = new.unsqueeze(0)
            shuffle_mat.append(new)

        bs_mat = torch.cat(shuffle_mat, dim=0)

        sim_match.append(bs_mat)

    final_feature = torch.stack(sim_match, dim=0)   #[bs, x, 768]?

    return final_feature, ind

def PFA(matrix, matrix1):
    '''
    @matrix shape [bs, 17, 768]
    @matrix1 shape [bs, 17, 768]

    '''
    assert matrix.shape[0] == matrix1.shape[0], 'Wrong shape'
    assert matrix.shape[1] == matrix1.shape[1], 'Wrong skt num'

    batch_size = matrix.shape[0] #[bs, 17, 768]

    # skt_num = matrix.shape[1]

    pose_weighted_feat = matrix * matrix1   #[bs, 17, 768]

    final_sim = F.cosine_similarity(matrix.unsqueeze(2), pose_weighted_feat.unsqueeze(1), dim=3) #[bs, 17, x]

    _, ind = torch.max(final_sim, dim=2)

    sim_match = []
    for i in range(batch_size):
        org_mat = matrix[i] #[17, C]
        sim_mat = pose_weighted_feat[i] #[17, C]
        shuffle_mat = []

        for j in range(ind.shape[1]):
            new = org_mat[j] + sim_mat[ind[i][j]]  #[C]
            new = new.unsqueeze(0)
            shuffle_mat.append(new)

        bs_mat = torch.cat(shuffle_mat, dim=0)

        sim_match.append(bs_mat)

    alignment_feat = torch.stack(sim_match, dim=0)   #[bs, 17, 768]?

    return alignment_feat

__factory_T_type = {
    'vit_base_patch16_224_TransReID': vit_base_patch16_224_TransReID
}

def make_pfd(cfg, num_class, camera_num, view_num):
    if cfg.MODEL.NAME == 'skeleton_transformer':
        if cfg.MODEL.JPM:
            model = build_skeleton_transformer(num_class, camera_num, view_num, cfg, __factory_T_type)
            print('===========building skeleton_transformer with PFA and PVM module ===========')
    else:
        raise RuntimeError('Not Support this model!')

    return model


#!/usr/bin/env python

"""
Script to control running all energy-infra algorithms.
"""

import os
import sys
import argparse
import shutil
from pathlib import Path
from multiprocessing import Pool

import numpy as np
import yaml

import geopandas as gpd
import rasterio
import accessestimator as ea
import gridfinder as gf

script_dir = Path(os.path.dirname(__file__))
with open(script_dir / "config.yml", "r") as ymlfile:
    cfg = yaml.safe_load(ymlfile)

admin_in = Path(cfg["inputs"]["admin"]).expanduser()
code = cfg["inputs"]["admin_code"]
ntl_in = Path(cfg["inputs"]["ntl_monthly"]).expanduser()
ntl_ann_in = Path(cfg["inputs"]["ntl_annual"]).expanduser()
pop_in = Path(cfg["inputs"]["pop"]).expanduser()
urban_in = Path(cfg["inputs"]["urban"]).expanduser()
pop_in = Path(cfg["inputs"]["pop"]).expanduser()

data = Path(cfg["outputs"]["base"]).expanduser()
scratch = Path(cfg["outputs"]["scratch"]).expanduser()

targets_dir = cfg["outputs"]["targets"]
costs_dir = cfg["outputs"]["costs"]
guess_dir = cfg["outputs"]["guess"]
vector_dir = cfg["outputs"]["vector"]
pop_elec_dir = cfg["outputs"]["pop_elec"]
local_dir = cfg["outputs"]["local"]

percentile = cfg["options"]["percentile"]
ntl_threshold = cfg["options"]["ntl_threshold"]
threads = cfg["options"]["threads"]
raise_errors = False

admin = gpd.read_file(admin_in)

def get_dirname(tool):
    if tool == targets:
        return targets_dir
    elif tool == costs:
        return costs_dir
    elif tool == dijk:
        return guess_dir
    elif tool == vector:
        return vector_dir
    elif tool == pop_elec:
        return pop_elec_dir
    elif tool == local:
        return local_dir
    else:
        return ValueError(f"{tool} not supported")

def get_filename(dirname, country, ext="tif"):
    return data / dirname / f"{country}.{ext}"

def get_filename_auto(tool, country):
    dirname = get_dirname(tool)
    ext = "tif"
    if tool == vector:
        ext = "gpkg"
    return get_filename(dirname, country, ext)

def spawn(tool, countries):
    if countries is None:
        countries = admin[code].tolist()

    countries[:] = [c for c in countries if not get_filename_auto(tool, c).is_file()]
    print("Will process", len(countries), "countries")

    with Pool(processes=threads) as pool:
        pool.map(tool, countries)

def targets(country):
    this_scratch = scratch / f"targets_{country}"
    ntl_out = this_scratch / "ntl"
    ntl_merged_out = this_scratch / "ntl_merged.tif"
    ntl_thresh_out = this_scratch / "ntl_thresh.tif"
    targets_out = get_filename(targets_dir, country)

    try:
        print(f"Targets\tstart\t{country}")
        this_scratch.mkdir(parents=True, exist_ok=True)
        aoi = admin.loc[admin[code] == country]
        buff = aoi.copy()
        buff.geometry = buff.buffer(0.1)

        # Clip NTL rasters and calculate nth percentile values
        gf.clip_rasters(ntl_in, ntl_out, buff)
        raster_merged, affine = gf.merge_rasters(ntl_out, percentile=percentile)
        gf.save_raster(ntl_merged_out, raster_merged, affine)

        # Apply filter to NTL
        ntl_filter = gf.create_filter()
        ntl_thresh, affine = gf.prepare_ntl(
            ntl_merged_out,
            buff,
            ntl_filter=ntl_filter,
            upsample_by=1,
            threshold=ntl_threshold,
        )
        gf.save_raster(ntl_thresh_out, ntl_thresh, affine)

        # Clip to actual AOI
        targets, affine, _ = gf.clip_raster(ntl_thresh_out, aoi)
        gf.save_raster(targets_out, targets, affine)
        msg = f"Targets\tDONE\t{country}"

    except Exception as e:
        msg = f"Targets\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        shutil.rmtree(this_scratch)
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

def costs(country):
    targets_in = get_filename(targets_dir, country)
    costs_in = get_filename("costs_vec", country, ext="gpkg")
    costs_out = get_filename(costs_dir, country)

    try:
        print(f"Costs\tstart\t{country}")
        aoi = admin.loc[admin[code] == country]
        roads_raster, affine = gf.prepare_roads(costs_in, aoi, targets_in)
        gf.save_raster(costs_out, roads_raster, affine, nodata=-1)

        msg = f"Costs\tDONE\t{country}"
    except Exception as e:
        msg = f"Costs\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

def dijk(country):
    this_scratch = scratch / f"dijk_{country}"
    dist_out = this_scratch / "dist.tif"
    targets_in = get_filename(targets_dir, country)
    costs_in = get_filename(costs_dir, country)
    guess_out = get_filename(guess_dir, country)

    try:
        print(f"Dijk\tstart\t{country}")
        this_scratch.mkdir(parents=True, exist_ok=True)

        targets, costs, start, affine = gf.get_targets_costs(targets_in, costs_in)
        dist = gf.optimise(targets, costs, start, silent=True)
        gf.save_raster(dist_out, dist, affine)
        guess, affine = gf.threshold(dist_out)
        guess_skel = gf.thin(guess)
        gf.save_raster(guess_out, guess_skel, affine)

        msg = f"Dijk\tDONE\t{country}"
    except Exception as e:
        msg = f"Dijk\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        shutil.rmtree(this_scratch)
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

def vector(country):
    guess_in = get_filename(guess_dir, country)
    guess_vec_out = get_filename(vector_dir, country, ext="gpkg")

    try:
        print(f"Vector\tstart\t{country}")
        guess_gdf = gf.raster_to_lines(guess_in)
        guess_gdf.to_file(guess_vec_out, driver="GPKG")

        msg = f"Vector\tDONE\t{country}"
    except Exception as e:
        msg = f"Vector\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

def pop_elec(country):
    targets_in = get_filename(targets_dir, country)
    pop_elec_out = get_filename(pop_elec_dir, country)
    weight_out = pop_elec_out.parents[0] / (str(pop_elec_out.stem) + "_W.tif")

    try:
        msg = ""
        print(f"\n\nPopElec\tstart\t{country}")
        aoi = admin.loc[admin[code] == country]
        access = aoi[["total", "urban", "rural"]].iloc[0].to_dict()
        if access["total"] == 1:
            return

        pop, urban, ntl, targets, affine, crs = ea.regularise(
            country, aoi, pop_in, urban_in, ntl_ann_in, targets_in
        )
        pop_elec, access_model_total, weights = ea.estimate(
            pop, urban, ntl, targets, access
        )
        gf.save_raster(pop_elec_out, pop_elec, affine, crs)
        # gf.save_raster(weight_out, weights, affine, crs)

        msg = f"PopElec\tDONE\t{country}\t\treal: {access['total']:.2f}\tmodel: {access_model_total:.2f}"
    except Exception as e:
        msg = f"PopElec\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

def local(country):
    pop_elec_in = get_filename(pop_elec_dir, country)
    lv_out = get_filename(local_dir, country)

    try:
        print(f"Local\tstart\t{country}")

        pop_elec_rd = rasterio.open(pop_elec_in)
        pop_elec = pop_elec_rd.read(1)
        affine = pop_elec_rd.transform
        crs = pop_elec_rd.crs

        aoi = admin.loc[admin[code] == country]
        access = aoi[["total", "urban", "rural"]].iloc[0].to_dict()
        peak_kw_pp = 0.1
        people_per_hh = 5
        if access["total"] >= 0.95:
            peak_kw_pp = 2
            people_per_hh = 3

        lengths = ea.apply_lv_length(
            pop_elec, peak_kw_pp=peak_kw_pp, people_per_hh=people_per_hh
        )
        gf.save_raster(lv_out, lengths, affine, crs)
        total_length = np.sum(lengths)
        msg = f"Local\tDONE\t{country}\tTot length: {total_length} km"
    except Exception as e:
        msg = f"Local\tFAILED\t{country}\t{e}"
        if raise_errors:
            raise
    finally:
        print(msg)
        if log:
            with open(log, "a") as f:
                print(msg, file=f)

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument(
        "tool", help="One of targets, cost, dijk, vector, pop_elec, local"
    )
    parser.add_argument("--countries")
    parser.add_argument("--targets_dir", default=targets_dir)
    parser.add_argument("--costs_dir", default=costs_dir)
    parser.add_argument("--guess_dir", default=guess_dir)
    parser.add_argument("--vector_dir", default=vector_dir)
    parser.add_argument("--pop_elec_dir", default=pop_elec_dir)
    parser.add_argument("--local_dir", default=local_dir)
    parser.add_argument("--percentile", default=percentile, type=int)
    parser.add_argument("--ntl_threshold", default=ntl_threshold, type=float)
    parser.add_argument(
        "-r",
        "--raise_errors",
        action="store_true",
        default=False,
        help="Whether to raise errors",
    )
    parser.add_argument(
        "-l",
        "--log",
        default=None,
        help="If supplied, logs will be written to this file",
    )
    args = parser.parse_args()

    switch = {
        "targets": targets,
        "costs": costs,
        "dijk": dijk,
        "vector": vector,
        "pop_elec": pop_elec,
        "local": local,
    }

    func = switch.get(args.tool)
    if func is None:
        sys.exit(f"Option {args.tool} not supported")

    countries = None
    if args.countries:
        if "," in args.countries:
            countries = args.countries.split(",")
        else:
            countries = [args.countries]

    targets_dir = args.targets_dir
    costs_dir = args.costs_dir
    guess_dir = args.guess_dir
    vector_dir = args.vector_dir
    pop_elec_dir = args.pop_elec_dir
    local_dir = args.local_dir
    percentile = args.percentile
    ntl_threshold = args.ntl_threshold
    raise_errors = args.raise_errors
    log = args.log

    spawn(func, countries)


"""
module for generating C, C++, Fortran77, Fortran90 and python routines that
evaluate sympy expressions. This module is work in progress. Only the
milestones with a '+' character in the list below have been completed.

--- How is sympy.utilities.codegen different from sympy.printing.ccode? ---

We considered the idea to extend the printing routines for sympy functions in
such a way that it prints complete compilable code, but this leads to a few
unsurmountable issues that can only be tackled with dedicated code generator:

- For C, one needs both a code and a header file, while the printing routines
generate just one string. This code generator can be extended to support .pyf
files for f2py.

- Sympy functions are not concerned with programming-technical issues, such as
input, output and input-output arguments. Other examples are contiguous or
non-contiguous arrays, including headers of other libraries such as gsl or others.

- It is highly interesting to evaluate several sympy functions in one C routine,
eventually sharing common intermediate results with the help of the cse routine.
This is more than just printing.

- From the programming perspective, expressions with constants should be
evaluated in the code generator as much as possible. This is different for
printing.

--- Basic assumptions ---

* A generic Routine data structure describes the routine that must be translated
  into C/Fortran/... code. This data structure covers all features present in
  one or more of the supported languages.

* Descendants from the CodeGen class transform multiple Routine instances into
  compilable code. Each derived class translates into a specific language.

* In many cases, one wants a simple workflow. The friendly functions in the last
  part are a simple api on top of the Routine/CodeGen stuff. They are easier to
  use, but are less powerful.

--- Milestones ---

+ First working version with scalar input arguments, generating C code, tests
+ Friendly functions that are easier to use than the rigorous Routine/CodeGen
  workflow.
+ Integer and Real numbers as input and output
- Optional extra include lines for libraries/objects that can eval special
  functions
- Test other C compilers and libraries: gcc, tcc, libtcc, gcc+gsl, ...
- Output arguments
- InputOutput arguments
- Sort input/output arguments properly
- Contiguous array arguments (sympy matrices)
- Non-contiguous array arguments (sympy matrices)
- ccode must raise an error when it encounters something that can not be
  translated into c. ccode(integrate(sin(x)/x, x)) does not make sense.
- Complex numbers as input and output
- Also generate .pyf code for f2py
- A default complex datatype
- Include extra information in the header: date, user, hostname, sha1 hash, ...
- Isolate constants and evaluate them beforehand in double precision
- Common Subexpression Elimination
- User defined comments in the generated code
- Fortran 77
- Fortran 90
- C++
- Python
- ...
"""

from sympy.core.symbol import Symbol
from sympy.core.expr import Expr
from sympy.core.containers import Tuple
from sympy.printing.ccode import ccode
from sympy.printing.fcode import fcode
from sympy.tensor import Idx, IndexedElement
from sympy.core.relational import Equality
from sympy.utilities import flatten

from StringIO import StringIO
import sympy
import os

__all__ = [
    # description of routines
    "Routine", "DataType", "default_datatypes", "get_default_datatype",
    "Argument", "InputArgument", "Result",
    # routines -> code
    "CodeGen", "CCodeGen", "FCodeGen",
    # friendly functions
    "codegen",
]

#
# Description of routines
#

class Routine(object):
    """Generic description of an evaluation routine for a set of sympy expressions.

       A CodeGen class can translate instances of this class into C/Fortran/...
       code. The routine specification covers all the features present in these
       languages. The CodeGen part must raise an exception when certain features
       are not present in the target language. For example, multiple return
       values are possible in Python, but not in C or Fortran. Another example:
       Fortran and Python support complex numbers, while C does not.
    """
    def __init__(self, name, expr):
        """Initialize a Routine instance.

           Arguments:
             name  --  A string with the name of this routine in the generated
                       code
             expr  --  The sympy expression that the Routine instance will represent.
                       If given a list or tuple of expressions, the routine
                       will be considered to have multiple return values.

            A decision about whether to use output arguments or return values,
            is made depending on the mathematical expressions.  For an expression
            of type Equality, the left hand side is made into an OutputArgument
            (or an InOutArgument if appropriate).  Else, the calculated
            expression is the return values of the routine.

            A tuple of exressions can be used to create a routine with both
            return value(s) and output argument(s).

        """
        arg_list = []

        if isinstance(expr, (list, tuple)):
            if not expr:
                raise ValueError("No expression given")
            expressions = Tuple(*expr)
        else:
            expressions = Tuple(expr)

        # local variables
        local_vars = set([i.label for i in expressions.atoms(Idx)])

        # symbols that should be arguments
        symbols = expressions.atoms(Symbol) - local_vars

        # Decide whether to use output argument or return value
        return_val = []
        output_args = []
        for expr in expressions:
            if isinstance(expr, Equality):
                out_arg = expr.lhs
                expr = expr.rhs
                if isinstance(out_arg, IndexedElement):
                    dims = out_arg.dimensions
                    symbol = out_arg.stem.label
                elif isinstance(out_arg, Symbol):
                    dims = []
                    symbol = out_arg
                else:
                    raise CodeGenError("Only IndexedElement or Symbol can define output arguments")

                if expr.has(symbol):
                    output_args.append(InOutArgument(symbol, out_arg, expr, dimensions=dims))
                else:
                    output_args.append(OutputArgument(symbol, out_arg, expr, dimensions=dims))

                # avoid duplicate arguments
                symbols.remove(symbol)
            else:
                return_val.append(Result(expr))

        # setup input argument list
        array_symbols = {}
        for array in expressions.atoms(IndexedElement):
            array_symbols[array.stem.label] = array

        for symbol in sorted(symbols):
            if symbol in array_symbols:
                dims = []
                array = array_symbols[symbol]
                for i in array.indices:
                    if i.lower == None:
                        dims.append((S.Zero, S.Zero))
                    else:
                        dims.append((i.lower, i.upper))
                metadata = {'dimensions': dims}
            else:
                metadata = {}

            arg_list.append(InputArgument(symbol, **metadata))

        arg_list.extend(output_args)

        self.name = name
        self.arguments = arg_list
        self.results = return_val
        self.local_vars = local_vars

    @property
    def result_variables(self):
        """Returns a tuple of OutputArgument, InOutArgument and Result."""
        args = [arg for arg in self.arguments if isinstance(arg, (OutputArgument, InOutArgument))]
        args.extend(self.results)
        return args

class DataType(object):
    """Holds strings for a certain datatype in different programming languages."""
    def __init__(self, cname, fname, pyname):
        self.cname = cname
        self.fname = fname
        self.pyname = pyname

default_datatypes = {
    "int": DataType("int", "INTEGER*4", "int"),
    "float": DataType("double", "REAL*8", "float")
}

def get_default_datatype(expr):
    """Derives a decent data type based on the assumptions on the expression."""
    if expr.is_integer:
        return default_datatypes["int"]
    else:
        return default_datatypes["float"]

class Argument(object):
    """An abstract Argument data structure: a name and a data type.

       This structure is refined in the descendants below.
    """

    def __init__(self, name, datatype=None, dimensions=None, precision=None):
        """Initialize an input argument.

           name  --  must be of class Symbol
           datatype  --  When not given, the data type will be guessed based
                         on the assumptions on the symbol argument.
           dimension  --  If present, the argument is interpreted as an array.
                          Dimensions must be a sequence containing tuples
                          of first and last index in the range.
           precision  --  FIXME
        """

        if not isinstance(name, Symbol):
            raise TypeError("The first argument must be a sympy symbol.")
        if datatype is None:
            datatype = get_default_datatype(name)
        elif not isinstance(datatype, DataType):
            raise TypeError("The (optional) `datatype' argument must be an instance of the DataType class.")
        if dimensions and not isinstance(dimensions, (tuple, list)):
            raise TypeError("The dimension argument must be a sequence of tuples")
        self.name = name
        self.datatype = datatype
        self.dimensions = dimensions
        self.precision = precision

    def get_symbols(self):
        """Returns a set of all symbols related to this argument.

        Scalar arguments return themselves in a set, while array arguments return
        the array variable as well as all symbols the specifiy dimensions.
        """
        if self.dimensions:
            symbs = set(flatten(self.dimensions))
            symbs.add(self.name)
            return symbs
        else:
            return set([self.name])

class InputArgument(Argument):
    pass

class ResultBase(object):

    @property
    def needs_initialization(self):
        return self._need_initialization

    def _prepare_expr(self):
        """Depending on the expression, it may need to be prepared for loops

        Example:

        The math expression denoting a matrix vector product is

        y(i) = A(i,j)*x(j)          (imlicit summation over j)

        Obviously, the code that calculates the matrix vector product must change
        the right hand side a little in order to store intermediate results
        correctly ....................
                                      |
        y = 0                         |
        do i=1,m                      |
            do j = 1,n                V
                y(i) = A(i,j)*x(i) + y(i)
            end do j
        end do i

        This would not be necessary if the left hand side has all indices that are on
        the right hand side.  E.g. for a dyadic product:

        do i=1,m
            do j = 1,n
                A(i,j) = x(i)*y(j)
            end do j
        end do i

        """
        rhs_loops = set([ i.label for i in self.expr.atoms(Idx) ])
        lhs_loops = set([ i.label for i in self.result_var.atoms(Idx) ])
        if rhs_loops - lhs_loops:
            self.expr = self.expr + self.result_var

class OutputArgument(Argument, ResultBase):
    """OutputArgument are always initialized in the routine
    """
    _need_initialization = True
    def __init__(self, name, result_var, expr, datatype=None, dimensions=None, precision=None):
        Argument.__init__(self, name, datatype, dimensions, precision)
        self.expr = expr
        self.result_var = result_var
        self._prepare_expr()

class InOutArgument(Argument, ResultBase):
    """InOutArgument are never initialized in the routine
    """
    _need_initialization = False

    def __init__(self, name, result_var, expr, datatype=None, dimensions=None, precision=None):
        Argument.__init__(self, name, datatype, dimensions, precision)
        self.expr = expr
        self.result_var = result_var

class Result(ResultBase):
    """An expression for a scalar return value.

       The name result is used to avoid conflicts with the reserved word
       'return' in the python language. It is also shorter than ReturnValue.
    """
    _need_initialization = False

    def __init__(self, expr, datatype=None):
        """Initialize a (scalar) return value.

           The second argument is optional. When not given, the data type will
           be guessed based on the assumptions on the expression argument.
        """
        if not isinstance(expr, Expr):
            raise TypeError("The first argument must be a sympy expression.")
        if datatype is None:
            datatype = get_default_datatype(expr)
        elif not isinstance(datatype, DataType):
            raise TypeError("The (optional) second argument must be an instance of the DataType class.")
        self.expr = expr
        self.datatype = datatype
        self.result_var = Symbol('result_%s'%hash(expr))
        self._prepare_expr()

#
# Transformation of routine objects into code
#

class CodeGen(object):
    """Abstract class for the code generators."""

    def __init__(self, project="project"):
        """Initialize a code generator.

           Derived classes will offer more options that affect the generated
           code.
        """
        self.project = project

    def write(self, routines, prefix, to_files=False, header=True, empty=True):
        """Writes all the source code files for the given routines.

           The generate source is returned as a list of (filename, contents)
           tuples, or is written to files (see options). Each filename consists
           of the given prefix, appended with an appropriate extension.

           Arguments:
             routines  --  A list of Routine instances to be written
             prefix  --  The prefix for the output files

           Optional arguments:
             to_files  --  When True, the output is effectively written to
                           files. [DEFAULT=False] Otherwise, a list of
                           (filename, contents) tuples is returned.
             header  --  When True, a header comment is included on top of each
                         source file. [DEFAULT=True]
             empty  --  When True, empty lines are included to structure the
                        source files. [DEFAULT=True]
        """
        if to_files:
            for dump_fn in self.dump_fns:
                filename = "%s.%s" % (prefix, dump_fn.extension)
                f = file(filename, "w")
                dump_fn(self, routines, f, prefix, header, empty)
                f.close()
        else:
            result = []
            for dump_fn in self.dump_fns:
                filename = "%s.%s" % (prefix, dump_fn.extension)
                contents = StringIO()
                dump_fn(self, routines, contents, prefix, header, empty)
                result.append((filename, contents.getvalue()))
            return result

    def dump_code(self, routines, f, prefix, header=True, empty=True):
        """Write the code file by calling language specific methods in correct order

           The generated file contains all the definitions of the routines in
           low-level code and refers to the header file if appropriate.

           Arguments:
             routines  --  a list of Routine instances
             f  --  a file-like object to write the file to
             prefix  --  the filename prefix, used to refer to the proper header
                         file. Only the basename of the prefix is used.

           Optional arguments:
             header  --  When True, a header comment is included on top of each
                         source file. [DEFAULT=True]
             empty  --  When True, empty lines are included to structure the
                        source files. [DEFAULT=True]
        """

        code_lines = []
        code_lines.extend(self._preprosessor_statements(prefix))
        for routine in routines:
            if empty: code_lines.append("\n")
            code_lines.extend(self._get_routine_opening(routine))
            code_lines.extend(self._declare_arguments(routine))
            code_lines.extend(self._declare_locals(routine))
            if empty: code_lines.append("\n")
            code_lines.extend(self._call_printer(routine))
            if empty: code_lines.append("\n")
            code_lines.extend(self._get_routine_ending(routine))

        if header and code_lines:
            self._dump_header(f)
        print >> f, ''.join(code_lines),

class CodeGenError(Exception):
    pass

header_comment = """Code generated with sympy %(version)s

See http://www.sympy.org/ for more information.

This file is part of '%(project)s'
"""

class CCodeGen(CodeGen):

    def _dump_header(self, f):
        """Writes a common header for the generated files."""
        print >> f, "/" + "*"*78
        tmp = header_comment % {"version": sympy.__version__, "project": self.project}
        for line in tmp.splitlines():
            print >> f, " *%s* " % line.center(76)
        print >> f, " " + "*"*78 + "/"

    def get_prototype_result(self, routine):
        """Returns a string for the function prototype for the given routine and
           a single result object, which can be None.

           If the routine has multiple result objects, an CodeGenError is
           raised.

           See: http://en.wikipedia.org/wiki/Function_prototype
        """
        prototype = []
        if len(routine.results) > 1:
            raise CodeGenError("C only supports a single or no return value.")
        elif len(routine.results) == 1:
            result = routine.results[0]
            prototype.append(result.datatype.cname)
        else:
            result = None
            prototype.append("void")
        # name of the routine + arguments + curly opening brackets
        prototype.append("%s(%s)" % (
            routine.name,
            ", ".join("%s %s" % (arg.datatype.cname, arg.name) for arg in routine.arguments)
        ))
        return " ".join(prototype), result

    def _preprosessor_statements(self, prefix):
        code_lines = []
        code_lines.append("#include \"%s.h\"\n" % os.path.basename(prefix))
        code_lines.append("#include <math.h>\n")
        return code_lines

    def _get_routine_opening(self, routine):
        prototype, result = self.get_prototype_result(routine)
        return ["%s {\n" % prototype]

    def _declare_arguments(self, routine):
        # arguments are in prototype
        return []

    def _declare_locals(self, routine):
        code_list = []
        for var in sorted(routine.local_vars, key=str):
            typeinfo = get_default_datatype(var)
            code_list.append("%s %s\n" % (typeinfo.cname, var))
        return code_list

    def _call_printer(self, routine):
        prototype, result = self.get_prototype_result(routine)
        if result is not None:
            return ["  return %s;\n" % ccode(result.expr)]
        else:
            return []

    def _get_routine_ending(self, routine):
        return ["}\n"]

    def dump_c(self, routines, f, prefix, header=True, empty=True):
        self.dump_code(routines, f, prefix, header, empty)
    dump_c.extension = "c"

    def dump_h(self, routines, f, prefix, header=True, empty=True):
        """Writes the C header file.

           This file contains all the function declarations.

           Arguments:
             routines  --  a list of Routine instances
             f  --  a file-like object to write the file to
             prefix  --  the filename prefix, used to construct the include
                         guards.

           Optional arguments:
             header  --  When True, a header comment is included on top of each
                         source file. [DEFAULT=True]
             empty  --  When True, empty lines are included to structure the
                        source files. [DEFAULT=True]
        """
        if header:
            self._dump_header(f)
        guard_name = "%s__%s__H" % (self.project.replace(" ", "_").upper(), prefix.replace("/", "_").upper())
        # include guards
        if empty: print >> f
        print >> f, "#ifndef %s" % guard_name
        print >> f, "#define %s" % guard_name
        if empty: print >> f
        # declaration of the function prototypes
        for routine in routines:
            prototype, result = self.get_prototype_result(routine)
            print >> f, "%s;" % prototype
        # end if include guards
        if empty: print >> f
        print >> f, "#endif"
        if empty: print >> f
    dump_h.extension = "h"

    # This list of dump functions is used by CodeGen.write to know which dump
    # functions it has to call.
    dump_fns = [dump_c, dump_h]

class FCodeGen(CodeGen):
    """
    Generator for Fortran 95 code
    """

    def __init__(self, project='project'):
        CodeGen.__init__(self, project)

    def _dump_header(self, f):
        """Writes a common header for the generated files."""
        print >> f, "!" + "*"*78
        tmp = header_comment % {"version": sympy.__version__, "project": self.project}
        for line in tmp.splitlines():
            print >> f, "!*%s* " % line.center(76)
        print >> f, "!" + "*"*78

    def _preprosessor_statements(self, prefix):
        return []

    def _get_routine_opening(self, routine):
        """
        Returns the opening statements of the fortran routine
        """
        code_list = []
        if len(routine.results) > 1:
            raise CodeGenError("Fortran only supports a single or no return value.")
        elif len(routine.results) == 1:
            result = routine.results[0]
            code_list.append(result.datatype.fname)
            code_list.append("function")
        else:
            code_list.append("subroutine")

        # name of the routine + arguments
        code_list.append("%s(%s)\n" % (routine.name,
            ", ".join("%s" % arg.name for arg in routine.arguments)))
        code_list = [ " ".join(code_list) ]

        code_list.append('implicit none\n')
        return code_list

    def _declare_arguments(self, routine):
        # argument type declarations
        code_list = []
        array_list = []
        scalar_list = []
        for arg in routine.arguments:

            if isinstance(arg, InputArgument):
                typeinfo = "%s, intent(in)" % arg.datatype.fname
            elif isinstance(arg, InOutArgument):
                typeinfo = "%s, intent(inout)" % arg.datatype.fname
            elif isinstance(arg, OutputArgument):
                typeinfo = "%s, intent(out)" % arg.datatype.fname
            else:
                raise CodeGenError("Unkown Argument type: %s"%type(arg))

            if arg.dimensions:
                # fortran arrays start at 1
                dimstr = ", ".join(["%s:%s"%(dim[0]+1, dim[1]+1)
                    for dim in arg.dimensions])
                typeinfo += ", dimension(%s)" % dimstr
                array_list.append("%s :: %s\n" % (typeinfo, arg.name))
            else:
                scalar_list.append("%s :: %s\n" % (typeinfo, arg.name))

        # scalars first, because they can be used in array declarations
        code_list.extend(scalar_list)
        code_list.extend(array_list)

        return code_list

    def _declare_locals(self, routine):
        code_list = []
        for var in sorted(routine.local_vars, key=str):
            typeinfo = get_default_datatype(var)
            code_list.append("%s :: %s\n" % (typeinfo.fname, var))

        return code_list

    def _get_routine_ending(self, routine):
        """
        Returns the closing statements of the fortran routine
        """
        if len(routine.results) == 1:
            return ["end function\n"]
        else:
            return ["end subroutine\n"]

    def get_interface(self, routine):
        """Returns a string for the function interface for the given routine and
           a single result object, which can be None.

           If the routine has multiple result objects, a CodeGenError is
           raised.

           See: http://en.wikipedia.org/wiki/Function_prototype

        """
        prototype = [ "interface\n" ]
        prototype.extend(self._get_routine_opening(routine))
        prototype.extend(self._declare_arguments(routine))
        prototype.extend(self._get_routine_ending(routine))
        prototype.append("end interface\n")

        return "".join(prototype)

    def _get_result(self, routine):
        """Returns a single result object, which can be return value or outargument
        """

        if len(routine.results) > 1:
            raise CodeGenError("Fortran only supports a single or no return value.")
        elif len(routine.results) == 1:
            result = routine.results[0]
        else:
            outargs = [ arg for arg in routine.arguments if
                    isinstance(arg, (OutputArgument, InOutArgument))]
            if len(outargs) == 1:
                result = outargs[0]
            else:
                raise CodeGenError("FIXME: need one and only one result object. Got %s"%len(outargs))

        return result

    def _init_resultvars(self, routine):
        """Returns codelines that intialize the result variables if applicable.
        """
        code_lines = []
        for arg in routine.arguments:
            if isinstance(arg, OutputArgument):
                if arg.datatype.fname == 'REAL*8':
                    code_lines.append("%s = 0.d0\n" % arg.name)
                elif arg.datatype.fname == 'INTEGER*4':
                    code_lines.append("%s = 0\n" % arg.name)
                else:
                    raise NotImplementedError
        if routine.results:
            code_lines.append("%s = 0.d0\n" % routine.name)

        return code_lines

    def _call_printer(self, routine):
        code_lines = []
        for result in routine.result_variables:
            if isinstance(result, Result):
                assign_to = routine.name
            elif isinstance(result, (OutputArgument, InOutArgument)):
                assign_to = result.result_var

            constants, not_fortran, f_expr = fcode(result.expr,
                assign_to=assign_to, source_format='free', human=False)
            code_lines.extend(constants)
            if result.needs_initialization:
                code_lines.extend(self._init_resultvars(routine))
            code_lines.append("%s\n" % f_expr)
        return code_lines

    def dump_f95(self, routines, f, prefix, header=True, empty=True):
        self.dump_code(routines, f, prefix, header, empty)
    dump_f95.extension = "f90"

    def dump_h(self, routines, f, prefix, header=True, empty=True):
        """Writes the interface  header file.

           This file contains all the function declarations.

           Arguments:
             routines  --  a list of Routine instances
             f  --  a file-like object to write the file to
             prefix  --  the filename prefix, used to construct the include
                         guards.

           Optional arguments:
             header  --  When True, a header comment is included on top of each
                         source file. [DEFAULT=True]
             empty  --  When True, empty lines are included to structure the
                        source files. [DEFAULT=True]
        """
        if header:
            self._dump_header(f)
        if empty: print >> f
        # declaration of the function prototypes
        for routine in routines:
            prototype  = self.get_interface(routine)
            print >> f, prototype,
        if empty: print >> f
    dump_h.extension = "h"

    # This list of dump functions is used by CodeGen.write to know which dump
    # functions it has to call.
    dump_fns = [dump_f95, dump_h]

def get_code_generator(language, project):
    CodeGenClass = {"C": CCodeGen, "F95": FCodeGen}.get(language.upper())
    if CodeGenClass is None:
        raise ValueError("Language '%s' is not supported." % language)
    return CodeGenClass(project)

#
# Friendly functions
#

def codegen(name_expr, language, prefix, project="project", to_files=False, header=True, empty=True):
    """Write source code for the given expressions in the given language.

       Mandatory Arguments:
         name_expr  --  A single (name, expression) tuple or a list of
                        (name, expression) tuples. Each tuple corresponds to a
                        routine.  If the expression is an equality (an
                        insance of class Equality) the left hand side is considered
                        an output argument.
         language  --  A string that indicates the source code language. This
                       is case insensitive. For the moment, only 'C' is
                       supported.
         prefix  --  A prefix for the names of the files that contain the source
                     code. Proper (language dependent) suffixes will be
                     appended.

       Optional Arguments:
         project  --  A project name, used for making unique preprocessor
                      instructions. [DEFAULT="project"]
         to_files  --  When True, the code will be written to one or more files
                       with the given prefix, otherwise strings with the names
                       and contents of these files are returned. [DEFAULT=False]
         header  --  When True, a header is written on top of each source file.
                     [DEFAULT=True]
         empty  --  When True, empty lines are used to structure the code.
                    [DEFAULT=True]

       >>> from sympy import symbols
       >>> from sympy.utilities.codegen import codegen
       >>> from sympy.abc import x, y, z
       >>> [(c_name, c_code), (h_name, c_header)] = \\
       ...     codegen(("f", x+y*z), "C", "test", header=False, empty=False)
       >>> print c_name
       test.c
       >>> print c_code,
       #include "test.h"
       #include <math.h>
       double f(double x, double y, double z) {
         return x + y*z;
       }
       >>> print h_name
       test.h
       >>> print c_header,
       #ifndef PROJECT__TEST__H
       #define PROJECT__TEST__H
       double f(double x, double y, double z);
       #endif

    """

    # Initialize the code generator.
    code_gen = get_code_generator(language, project)

    # Construct the routines based on the name_expression pairs.
    #  mainly the input arguments require some work
    routines = []
    if isinstance(name_expr[0], basestring):
        # single tuple is given, turn it into a singleton list with a tuple.
        name_expr = [name_expr]

    for name, expr in name_expr:
        routines.append(Routine(name, expr))

    # Write the code.
    return code_gen.write(routines, prefix, to_files, header, empty)