import random
import torch
import torch.nn as nn
import torch.nn.functional as F
from torch.autograd import Variable
import torchvision.models as models
from torch.autograd import Variable
import numpy as np
from model.utils.config import cfg
from model.rpn.rpn import _RPN
from model.roi_pooling.modules.roi_pool import _RoIPooling
from model.roi_crop.modules.roi_crop import _RoICrop
from model.roi_align.modules.roi_align import RoIAlignAvg
from model.rpn.proposal_target_layer_cascade import _ProposalTargetLayer
import time
import pdb
from model.utils.net_utils import _smooth_l1_loss, _crop_pool_layer, _affine_grid_gen, _affine_theta

name_vectors = {0:[float(x) for x in "-0.6742 0.85137 -0.44563 -0.13542 0.66321 0.40091 -0.17317 -0.31843 -0.50706 -0.16611 -0.0037329 -0.15644 0.49711 0.42111 -0.15725 -0.29234 -0.49116 -0.41583 0.40411 -0.98499 0.038447 0.58983 -0.097437 0.13938 0.21717 -0.79006 -1.1404 0.063862 0.091416 0.13035 2.6491 -0.39185 0.10433 -1.2945 0.25436 0.68762 -0.24406 -0.55442 -0.26576 -0.19875 0.4121 0.48016 0.10084 -0.0076807 0.14454 -0.2255 0.78955 0.49908 0.1241 0.06661".split()],
                1:[float(x) for x in "0.27203 -0.83471 -0.77436 -0.52342 -0.7846 0.43548 -0.55723 -0.12461 0.40705 -0.26957 0.9665 0.65333 -0.16101 -0.12138 -1.2511 -0.3532 -0.99238 1.3659 -1.4554 -0.61021 0.2708 0.23935 -0.68575 -0.38805 0.31965 -0.43329 -0.56755 0.32242 0.7346 0.21644 0.022579 -0.34303 -0.19963 1.0519 0.80017 -0.27862 0.70645 0.18759 -0.46561 0.033631 0.9176 -1.2649 0.23333 0.1328 0.32079 0.038267 0.2414 0.0043794 0.046835 0.20238".split()],
                2:[float(x) for x in "-0.41716 0.16271 0.18208 0.040874 -0.76743 0.47832 -0.95683 -0.69713 1.3446 -0.59146 0.49005 -0.59504 -0.96514 0.41526 -0.053253 -0.65851 -0.33963 1.4676 -0.51452 -1.5919 0.44235 -0.10577 -1.4277 1.1811 0.2842 -0.92328 -0.2138 0.10129 0.61985 -0.21404 1.3418 0.53977 -0.65476 0.55278 0.38827 0.34769 0.37724 0.45527 -0.30526 0.97048 -0.016631 -0.36535 0.060563 0.33608 0.098575 -0.96056 0.31872 -1.1316 0.91703 -0.65762".split()],
                3:[float(x) for x in "0.78675 0.079368 -0.76597 0.1931 0.55014 0.26493 -0.75841 -0.8818 1.6468 -0.54381 0.33519 0.44399 1.089 0.27044 0.74471 0.2487 0.2491 -0.28966 -1.4556 0.35605 -1.1725 -0.49858 0.35345 -0.1418 0.71734 -1.1416 -0.038701 0.27515 -0.017704 -0.44013 1.9597 -0.064666 0.47177 -0.03 -0.31617 0.26984 0.56195 -0.27882 -0.36358 -0.21923 -0.75046 0.31817 0.29354 0.25109 1.6111 0.7134 -0.15243 -0.25362 0.26419 0.15875".split()],
                4:[float(x) for x in "0.96989 0.075746 0.157 -0.10599 -0.6147 0.085639 -1.3175 0.63774 0.84952 -1.0661 0.023076 0.69078 0.49954 0.89659 -0.11993 -0.68484 -0.055934 0.89771 -1.6127 -0.93314 0.34222 0.56598 -0.49005 0.064676 1.27 -1.5572 0.47292 0.36106 0.58891 -0.25283 2.1768 0.029947 -0.45634 1.1527 0.38669 0.26368 0.32151 -0.50073 -0.34753 0.094688 -0.10259 -0.34041 1.2957 0.019886 0.056735 -0.68682 -0.24096 -0.91495 0.73288 -0.95561".split()],
                5:[float(x) for x in "0.20635 0.2606 -0.094723 -0.73396 0.72598 0.5099 -0.39352 -0.45703 0.49335 1.3791 0.10285 0.14997 0.41506 -0.19039 1.0527 0.16514 -0.16717 0.8092 -0.97394 -1.753 0.34632 -0.053064 0.33046 -0.021036 -0.78655 -1.0088 -0.30341 1.6766 0.90808 -0.39309 1.2131 0.21588 -0.87778 1.3756 0.57432 0.35111 0.39926 0.33184 1.2035 -0.21218 1.2316 0.58557 -0.40531 0.37376 0.16584 0.56948 -0.13898 -0.29062 0.56082 -0.94112".split()],
                6:[float(x) for x in "0.84772 0.070253 0.96791 -0.27164 -0.37617 0.31978 -1.3108 0.091093 0.59919 -0.90217 -0.050876 -0.83886 -0.61596 0.29642 -0.42189 -0.21969 -0.94006 1.2221 -0.66526 -0.57745 0.76126 0.51459 -0.88565 1.5135 0.42326 -1.2947 0.45522 0.67073 0.80188 -0.65449 2.4117 0.62445 -0.046631 0.37524 1.0103 0.25259 1.0913 -0.79427 -0.17027 1.4866 -0.24077 0.021904 -0.01603 -0.44319 -0.13914 0.013311 -0.49432 -0.57696 1.1997 -0.39707".split()],
                7:[float(x) for x in "0.47685 -0.084552 1.4641 0.047017 0.14686 0.5082 -1.2228 -0.22607 0.19306 -0.29756 0.20599 -0.71284 -1.6288 0.17096 0.74797 -0.061943 -0.65766 1.3786 -0.68043 -1.7551 0.58319 0.25157 -1.2114 0.81343 0.094825 -1.6819 -0.64498 0.6322 1.1211 0.16112 2.5379 0.24852 -0.26816 0.32818 1.2916 0.23548 0.61465 -0.1344 -0.13237 0.27398 -0.11821 0.1354 0.074306 -0.61951 0.45472 -0.30318 -0.21883 -0.56054 1.1177 -0.36595".split()],
                8:[float(x) for x in "0.45281 -0.50108 -0.53714 -0.015697 0.22191 0.54602 -0.67301 -0.6891 0.63493 -0.19726 0.33685 0.7735 0.90094 0.38488 0.38367 0.2657 -0.08057 0.61089 -1.2894 -0.22313 -0.61578 0.21697 0.35614 0.44499 0.60885 -1.1633 -1.1579 0.36118 0.10466 -0.78325 1.4352 0.18629 -0.26112 0.83275 -0.23123 0.32481 0.14485 -0.44552 0.33497 -0.95946 -0.097479 0.48138 -0.43352 0.69455 0.91043 -0.28173 0.41637 -1.2609 0.71278 0.23782".split()],
                9:[float(x) for x in "-1.0443 0.49202 -0.75978 -0.39224 0.81217 -0.039287 0.016706 -0.68629 -0.078359 -1.3214 -0.15354 0.20438 -0.46503 1.2145 -0.18217 0.27451 -0.24086 0.71145 0.3247 -0.7132 0.66721 0.71307 -0.10394 -0.38439 -0.2026 -1.4419 0.42644 0.59436 -1.3615 0.0013784 1.8734 -0.11334 -0.88115 -0.21715 -0.56606 0.14152 0.27673 0.99962 1.0567 -0.29428 -0.3139 0.12729 -0.54363 0.39652 -0.32527 0.30536 0.15128 -1.0889 -0.20867 -0.052605".split()],
                10:[float(x) for x in "0.61253 -0.48167 -0.74199 -0.55203 -0.007596 1.6101 -0.88565 -0.81981 1.5144 -0.22804 0.55367 -0.18392 0.7049 -0.36931 1.0668 1.1077 0.19709 0.24731 -0.68395 0.5475 -0.038255 -0.78989 0.61131 0.31473 0.50215 -1.6535 -0.42782 1.0404 0.29429 -0.36889 1.3148 -0.18443 0.092753 0.77572 -0.54845 -0.14645 0.51128 0.047248 0.41781 -0.18324 -0.44197 -0.25237 -0.3359 0.3096 1.9192 0.3396 -0.27341 -0.01316 0.64974 -0.85857".split()],
                11:[float(x) for x in "0.08052    2.4489    -1.82518   -0.0477     2.00389   -1.69875 -0.751511  -0.50848   -0.428588  -1.31098   -1.68757   -0.32697  0.07998    1.65463    0.90282    0.26249    0.13324    0.62698  0.371472  -2.6305     1.60213    0.56334   -0.38732    1.69456 -0.58381   -0.632158  -0.41       1.51932   -0.09594    0.21045  4.788      0.99336   -0.44077    0.63905    0.6379     1.42181  0.47585    1.95477    0.327167  -0.12653    0.77951   -0.10258 -0.179286   2.03756    0.1777877  0.9459     0.39777   -0.778347  0.99945   -0.651".split()],
                12:[float(x) for x in "0.11008 -0.38781 -0.57615 -0.27714 0.70521 0.53994 -1.0786 -0.40146 1.1504 -0.5678 0.0038977 0.52878 0.64561 0.47262 0.48549 -0.18407 0.1801 0.91397 -1.1979 -0.5778 -0.37985 0.33606 0.772 0.75555 0.45506 -1.7671 -1.0503 0.42566 0.41893 -0.68327 1.5673 0.27685 -0.61708 0.64638 -0.076996 0.37118 0.1308 -0.45137 0.25398 -0.74392 -0.086199 0.24068 -0.64819 0.83549 1.2502 -0.51379 0.04224 -0.88118 0.7158 0.38519".split()],
                13:[float(x) for x in "-0.20454 0.23321 -0.59158 -0.29205 0.29391 0.31169 -0.94937 0.055974 1.0031 -1.0761 -0.0094648 0.18381 -0.048405 -0.35717 0.26004 -0.41028 0.51489 1.2009 -1.6136 -1.1003 -0.23455 -0.81654 -0.15103 0.37068 0.477 -1.7027 -1.2183 0.038898 0.23327 0.028245 1.6588 0.26703 -0.29938 0.99149 0.34263 0.15477 0.028372 0.56276 -0.62823 -0.67923 -0.163 -0.49922 -0.8599 0.85469 0.75059 -1.0399 -0.11033 -1.4237 0.65984 -0.3198".split()],
                14:[float(x) for x in "0.14362 -1.1402 0.39368 0.18135 -0.094088 0.67473 -0.52618 0.21466 0.62416 -0.17217 0.67109 -1.1389 -0.84819 0.085305 0.20975 -0.59836 -0.78554 1.21 -0.90412 -1.009 0.42731 0.39614 -1.0663 0.66758 0.54771 -0.93963 -0.31805 0.14893 0.4489 -0.1986 0.20147 0.47226 -0.31627 0.83248 0.84036 0.40339 0.24902 -0.034884 -0.11794 0.89527 -0.33927 0.13761 -0.037933 -0.26963 0.85965 -1.174 0.31216 -0.62433 1.4447 -1.0968".split()],
                15:[float(x) for x in "0.61734 0.40035 0.067786 -0.34263 2.0647 0.60844 0.32558 0.3869 0.36906 0.16553 0.0065053 -0.075674 0.57099 0.17314 1.0142 -0.49581 -0.38152 0.49255 -0.16737 -0.33948 -0.44405 0.77543 0.20935 0.6007 0.86649 -1.8923 -0.37901 -0.28044 0.64214 -0.23549 2.9358 -0.086004 -0.14327 -0.50161 0.25291 -0.065446 0.60768 0.13984 0.018135 -0.34877 0.039985 0.07943 0.39318 1.0562 -0.23624 -0.4194 -0.35332 -0.15234 0.62158 0.79257".split()],
                16:[float(x) for x in "1.87558   0.70306  -0.13368   1.0749    0.344     0.359    -1.29974 -0.81254   0.38347   0.20391   0.19335  -0.2915    1.75308  -1.23573  1.04908   0.20029   0.01158   2.52851  -0.17232  -1.70577  -0.50728 -2.48107  -0.60158  -1.52139   0.59061  -0.46542   1.71796   1.25714  1.44733  -0.97309   2.35745  -1.07286  -0.67348  -0.21713   0.02439  0.835637 -0.481641  2.17821   1.30031   0.16752  -0.37618  -1.94368  0.213935 -0.00917   2.51475   1.63038  -0.311639 -1.106158 -0.94201 -0.58308".split()],
                17:[float(x) for x in "0.39026 -1.1357 -0.47646 -1.0185 0.44893 1.4499 -1.5251 -0.97275 1.5137 -0.58559 0.68279 -0.63901 1.6961 -1.233 0.50333 0.6791 1.0007 0.53626 -1.1749 -0.054361 -0.42404 -0.49371 0.43183 0.40727 0.74916 -0.53304 -0.33745 0.11388 0.47025 -0.47616 1.3084 0.1534 0.09754 1.0811 -0.20525 0.14341 -0.080796 0.17984 0.12433 0.12709 -0.31428 0.035969 -0.4609 0.53133 1.4986 -0.22609 -0.46637 -0.54343 -0.27696 -0.75601".split()],
                18:[float(x) for x in "-0.12335 0.23509 -0.7438 -0.50219 0.97023 -0.12771 0.039666 -0.67514 -0.2298 -1.2603 -0.18912 0.019617 -0.096285 1.3191 0.31878 1.3884 -0.41529 0.33527 0.94093 -0.50369 0.37852 1.104 -0.41287 -0.10135 -0.59901 0.060218 -0.21657 1.5896 0.57137 0.061694 0.66554 0.39239 -0.73251 1.401 -0.095607 0.66528 0.36132 -0.014117 0.068288 -0.62261 -0.023754 -0.087556 0.1543 0.5651 -1.0085 0.065175 0.2977 -1.0227 -0.21718 -0.49374".split()],
                19:[float(x) for x in "0.94971 0.34328 0.84504 -0.88519 -0.72078 -0.29309 -0.74678 0.65122 0.47295 -0.74011 0.1877 -0.38279 -0.55899 0.42952 -0.26984 -0.42383 -0.31236 1.3423 -0.78567 -0.6302 0.91819 0.21126 -0.57442 1.4549 0.75456 -1.6165 -0.0085015 0.0029134 0.51304 -0.47447 2.5306 0.85944 -0.30667 0.057765 0.66231 0.20804 0.64237 -0.5246 -0.053416 1.1404 -0.13703 -0.18361 0.45459 -0.50963 -0.025539 -0.02861 0.18048 -0.4483 0.40525 -0.36821".split()],
                20:[float(x) for x in "0.25626    -0.17277     1.52866     1.06081    -0.56051    -1.009539 -0.845252   -1.18025     1.38689    -0.24081     0.622749    0.121095  0.02118     2.1764      0.712973    1.2033     -0.87334     0.5934 -0.11       -0.34415     1.34936     1.39815     1.1527      1.63682  0.0212764  -2.23525    -0.977023    0.09278    -0.01013    -0.06802  5.4253      0.57827    -0.6189     -1.3413     -1.44589     0.63279  0.43006    -1.66446    -0.96338    -0.87314     0.99411     1.098152 -0.24369    -0.4259232  -0.697979   -0.101647   -0.04148     0.58366 -0.26210906  0.947361".split()]}

def scale_invariant_loss(t1,t2):
    return torch.pow(torch.sum(t1-t2, dim=1),2)/2500.

class _fasterRCNN(nn.Module):
    """ faster RCNN """
    def __init__(self, classes, class_agnostic, wvsize=50,
                 ce_loss=True, mse_loss=False, cosine_loss=False, norm_cosine_loss=False):
        super(_fasterRCNN, self).__init__()
        self.classes = classes
        self.n_classes = len(classes)
        self.class_agnostic = class_agnostic
        # loss
        self.RCNN_loss_cls = 0
        self.RCNN_loss_bbox = 0

        # define rpn
        self.RCNN_rpn = _RPN(self.dout_base_model)
        self.RCNN_proposal_target = _ProposalTargetLayer(self.n_classes)
        self.RCNN_roi_pool = _RoIPooling(cfg.POOLING_SIZE, cfg.POOLING_SIZE, 1.0/16.0)
        self.RCNN_roi_align = RoIAlignAvg(cfg.POOLING_SIZE, cfg.POOLING_SIZE, 1.0/16.0)

        self.grid_size = cfg.POOLING_SIZE * 2 if cfg.CROP_RESIZE_WITH_MAX_POOL else cfg.POOLING_SIZE
        self.RCNN_roi_crop = _RoICrop()

        self.wv_size = wvsize
        self.ce_loss = ce_loss
        self.mse_loss = mse_loss
        self.cosine_loss = cosine_loss
        self.norm_cosine_loss = norm_cosine_loss

    def forward(self, im_data, im_info, gt_boxes, num_boxes):
        batch_size = im_data.size(0)

        im_info = im_info.data
        gt_boxes = gt_boxes.data
        num_boxes = num_boxes.data

        # feed image data to base model to obtain base feature map
        base_feat = self.RCNN_base(im_data)

        # feed base feature map tp RPN to obtain rois
        rois, rpn_loss_cls, rpn_loss_bbox = self.RCNN_rpn(base_feat, im_info, gt_boxes, num_boxes)

        # if it is training phrase, then use ground trubut bboxes for refining
        if self.training:
            roi_data = self.RCNN_proposal_target(rois, gt_boxes, num_boxes)
            rois, rois_label, rois_target, rois_inside_ws, rois_outside_ws = roi_data

            rois_label = Variable(rois_label.view(-1).long())
            rois_target = Variable(rois_target.view(-1, rois_target.size(2)))
            rois_inside_ws = Variable(rois_inside_ws.view(-1, rois_inside_ws.size(2)))
            rois_outside_ws = Variable(rois_outside_ws.view(-1, rois_outside_ws.size(2)))
        else:
            rois_label = None
            rois_target = None
            rois_inside_ws = None
            rois_outside_ws = None
            rpn_loss_cls = 0
            rpn_loss_bbox = 0

        rois = Variable(rois)
        # do roi pooling based on predicted rois

        if cfg.POOLING_MODE == 'crop':
            # pdb.set_trace()
            # pooled_feat_anchor = _crop_pool_layer(base_feat, rois.view(-1, 5))
            grid_xy = _affine_grid_gen(rois.view(-1, 5), base_feat.size()[2:], self.grid_size)
            grid_yx = torch.stack([grid_xy.data[:,:,:,1], grid_xy.data[:,:,:,0]], 3).contiguous()
            pooled_feat = self.RCNN_roi_crop(base_feat, Variable(grid_yx).detach())
            if cfg.CROP_RESIZE_WITH_MAX_POOL:
                pooled_feat = F.max_pool2d(pooled_feat, 2, 2)
        elif cfg.POOLING_MODE == 'align':
            pooled_feat = self.RCNN_roi_align(base_feat, rois.view(-1, 5))
        elif cfg.POOLING_MODE == 'pool':
            pooled_feat = self.RCNN_roi_pool(base_feat, rois.view(-1,5))

        # feed pooled features to top model
        pooled_feat = self._head_to_tail(pooled_feat)

        # compute bbox offset
        bbox_pred = self.RCNN_bbox_pred(pooled_feat)
        if self.training and not self.class_agnostic:
            # select the corresponding columns according to roi labels
            bbox_pred_view = bbox_pred.view(bbox_pred.size(0), int(bbox_pred.size(1) / 4), 4)
            bbox_pred_select = torch.gather(bbox_pred_view, 1, rois_label.view(rois_label.size(0), 1, 1).expand(rois_label.size(0), 1, 4))
            bbox_pred = bbox_pred_select.squeeze(1)

        if self.ce_loss:
        # compute object classification probability with ce_loss
            cls_score = self.RCNN_cls_score(pooled_feat)
#            print("computed cls_score")
#        print(self.mse_loss, self.cosine_loss, self.norm_cosine_loss)
        if self.mse_loss or self.cosine_loss or self.norm_cosine_loss:
            cls_score_wv = self.RCNN_cls_score_wv(pooled_feat)
#            print("computed cls_score_wv")

#        tanh_layer = nn.Tanh()
#        cls_score = tanh_layer(cls_score)

        cls_prob = F.softmax(cls_score, 1)

        RCNN_loss_cls = 0
        RCNN_loss_bbox = 0
        RCNN_loss_cls_wv = 0
        nonzero_cls_loss_wv = torch.tensor(0.).cuda()
        losses = {}

        if self.training:
            # bounding box regression L1 loss
            RCNN_loss_bbox = _smooth_l1_loss(bbox_pred, rois_target, rois_inside_ws, rois_outside_ws)
            losses['rpn_loss_cls'] = rpn_loss_cls.unsqueeze(0)
            losses['rpn_loss_bbox'] = rpn_loss_bbox.unsqueeze(0)
            losses['RCNN_loss_bbox'] = RCNN_loss_bbox.unsqueeze(0)

#            print("Is training:",self.ce_loss, self.mse_loss, self.cosine_loss, self.norm_cosine_loss)
            if self.ce_loss:
                RCNN_loss_cls = self.original_cls_loss_fun(cls_score, rois_label)
                losses['RCNN_loss_cls'] = RCNN_loss_cls.unsqueeze(0)

            if self.mse_loss or self.cosine_loss or self.norm_cosine_loss:
                # create a vector for the size of the word vector
                rois_vector = torch.zeros(rois_label.shape[0], self.wv_size)
                for ind, val in enumerate(rois_label):
                    rois_vector[ind] = torch.tensor(name_vectors[val.item()])
                rois_vector = rois_vector.cuda()

                if self.mse_loss:
                    nonzero_cls_loss_wv, RCNN_loss_cls_wv = self.mse_loss_fun(cls_score_wv, rois_label, rois_vector)
                    losses['RCNN_loss_cls_wv'] = RCNN_loss_cls_wv.unsqueeze(0)
                elif self.cosine_loss:
                    nonzero_cls_loss_wv, RCNN_loss_cls_wv = self.cosine_loss_fun(cls_score_wv, rois_vector)
                    losses['RCNN_loss_cls_wv'] = RCNN_loss_cls_wv.unsqueeze(0)
                elif self.norm_cosine_loss:
                    nonzero_cls_loss_wv, RCNN_loss_cls_wv = self.norm_cosine_loss_fun(cls_score_wv, rois_vector)
                    losses['RCNN_loss_cls_wv'] = RCNN_loss_cls_wv.unsqueeze(0)
                else:
                    print("no word vector loss added")

        cls_prob = cls_prob.view(batch_size, rois.size(1), -1)
        bbox_pred = bbox_pred.view(batch_size, rois.size(1), -1)

        if self.training:
            return rois, nonzero_cls_loss_wv.unsqueeze(0), bbox_pred, losses, rois_label
#            return rois, nonzero_cls_loss_wv.unsqueeze(0), bbox_pred, rpn_loss_cls.unsqueeze(0), rpn_loss_bbox.unsqueeze(0), RCNN_loss_cls.unsqueeze(0), RCNN_loss_bbox.unsqueeze(0), rois_label
        else:
            return  rois, cls_score, cls_score_wv, bbox_pred, losses, rois_label

    def original_cls_loss_fun(self, cls_score, rois_label):
        RCNN_loss_cls = F.cross_entropy(cls_score, rois_label)
        return RCNN_loss_cls

    def norm_cosine_loss_fun(self, cls_score, rois_vector):
        cls_score = F.normalize(cls_score, p=2, dim=1)
        rois_vector = F.normalize(rois_vector, p=2, dim=1)
        RCNN_loss_cls_wv = 1 - F.cosine_similarity(cls_score, rois_vector)
        return torch.tensor(0.).cuda(), RCNN_loss_cls_wv

    def cosine_loss_fun(self, cls_score, rois_vector):
        RCNN_loss_cls_wv = 1 - F.cosine_similarity(cls_score, rois_vector)
        return torch.tensor(0.).cuda(), RCNN_loss_cls_wv # the first argument is for the nonzero_cls_loss_wv that we currently dont have in cosine loss, so that the code wont break

    #TODO: make the mse loss "50" invariant, hint check scale invariant loss
    def mse_loss_fun(self, cls_score, rois_label, rois_vector):
        mse_loss_fun = nn.MSELoss(reduce=False)

        mse_loss_notmean = mse_loss_fun(cls_score, rois_vector)
        mse_loss = torch.mean(mse_loss_notmean, dim=1)
        smse_loss = scale_invariant_loss(cls_score, rois_vector)
        scale_loss = mse_loss - smse_loss

        indexes = np.where(rois_label != 0)
        # nonzero_cls_loss_wv is only for output, we dont use it for backpropagation
        nonzero_cls_loss_wv = torch.tensor(np.mean(scale_loss[indexes].detach().cpu().numpy()), dtype=torch.float64, requires_grad=False).cuda()
        RCNN_loss_cls_wv = torch.mean(scale_loss)

        return nonzero_cls_loss_wv, RCNN_loss_cls_wv

    def _init_weights(self):
        def normal_init(m, mean, stddev, truncated=False):
            """
            weight initalizer: truncated normal and random normal.
            """
            # x is a parameter
            if truncated:
                m.weight.data.normal_().fmod_(2).mul_(stddev).add_(mean) # not a perfect approximation
            else:
                m.weight.data.normal_(mean, stddev)
                m.bias.data.zero_()

        normal_init(self.RCNN_rpn.RPN_Conv, 0, 0.01, cfg.TRAIN.TRUNCATED)
        normal_init(self.RCNN_rpn.RPN_cls_score, 0, 0.01, cfg.TRAIN.TRUNCATED)
        normal_init(self.RCNN_rpn.RPN_bbox_pred, 0, 0.01, cfg.TRAIN.TRUNCATED)
        if self.ce_loss:
            normal_init(self.RCNN_cls_score, 0, 0.01, cfg.TRAIN.TRUNCATED)
            print("DEBUG: self.RCNN_cls_score weights initialized")
        if self.mse_loss or self.cosine_loss or self.norm_cosine_loss:
            normal_init(self.RCNN_cls_score_wv, 0, 0.01, cfg.TRAIN.TRUNCATED)
            print("DEBUG: self.RCNN_cls_score_wv weights initialized")
        normal_init(self.RCNN_bbox_pred, 0, 0.001, cfg.TRAIN.TRUNCATED)

    def create_architecture(self):
        self._init_modules()
        self._init_weights()


"""
Created on Thu Jan 17 14:04:02 2019

face recognize api

@author: zyb_as
"""
import os
import numpy as np
from scipy import misc
import tensorflow as tf

import sys_config
import facenet
from align import detect_face

class face_recognizer(object):
    ''' A recognizer for performing face detection an recognition

    base on facenet tensorflow implementation: https://github.com/davidsandberg/facenet
    use mtcnn to do face detection
    use pretrained facenet to do face recognition
    '''

    def __init__(self):
        '''load mtcnn and facenet model

        load all needed parameters from sys_config.py
        preload the models and save correspond session
        '''
        self.facenet_weight_dir = sys_config.facenet_weight_dir
        self.face_minsize = sys_config.face_minsize
        self.image_size = sys_config.image_size
        self.margin = sys_config.margin
        self.three_threshold = sys_config.three_threshold
        self.factor = sys_config.factor
        self.distance_threshold = sys_config.distance_threshold

        print('Creating networks and loading parameters')
        with tf.Graph().as_default():
            # setting not fully occupied memory, allocated on demand
            config = tf.ConfigProto()
            config.gpu_options.allow_growth = True
            sess = tf.Session(config = config)
            with sess.as_default():
                # load mtcnn model(do face detection and align)
                self.pnet, self.rnet, self.onet = detect_face.create_mtcnn(sess, None)

                # load the facenet model(do face recognition)
                facenet.load_model(self.facenet_weight_dir)

                # Get input and output tensors
                self.images_placeholder = tf.get_default_graph().get_tensor_by_name("input:0")
                self.embeddings = tf.get_default_graph().get_tensor_by_name("embeddings:0")
                self.phase_train_placeholder = tf.get_default_graph().get_tensor_by_name("phase_train:0")

                # save session
                self.facenet_session = sess

                # load target people feature embeddings
                template_embeddings_file = sys_config.template_face_embeddings_file
                if os.path.exists(template_embeddings_file):
                    self.template_face_embeddings = np.load(template_embeddings_file)
        print("init finish")

    def _compute_distance(self, embedding1, embedding2):
        #distance = np.linalg.norm(feature1 - feature2)
        distance = np.sqrt(np.sum(np.square(np.subtract(embedding1, embedding2))))
        return distance

    def detect_face_and_align(self, img):
        img_size = np.asarray(img.shape)[0:2]
        #face_minsize = int(min(img_size[0], img_size[1]) * 0.15)
        #face_minsize = max(face_minsize, self.face_minsize)
        bounding_boxes, _ = detect_face.detect_face(img, self.face_minsize,
            self.pnet, self.rnet, self.onet, self.three_threshold, self.factor)
        if len(bounding_boxes) < 1:
            return np.array([])

        bb_list = []
        for i in range(len(bounding_boxes)):
            #det = np.squeeze(bounding_boxes[i, 0:4])
            det = bounding_boxes[i, 0:4]
            bb = np.zeros(4, dtype=np.int32)
            bb[0] = np.maximum(det[0]-self.margin/2, 0)
            bb[1] = np.maximum(det[1]-self.margin/2, 0)
            bb[2] = np.minimum(det[2]+self.margin/2, img_size[1])
            bb[3] = np.minimum(det[3]+self.margin/2, img_size[0])
            bb_list.append(bb)

        face_list = []
        for i in range(len(bb_list)):
            bb = bb_list[i]
            cropped = img[bb[1]:bb[3], bb[0]:bb[2], :]
            aligned = misc.imresize(cropped, (self.image_size, self.image_size), interp='bilinear')
            prewhitened = facenet.prewhiten(aligned)
            face_list.append(prewhitened)
        face_array = np.stack(face_list)
        return face_array

    def extract_face_embedding(self, faces):
        if len(faces) < 1:
            print("warning: input parameter faces's length < 1")
            return None

        if isinstance(faces, list):
            faces = np.array(faces)  # convert into ndarray

        with tf.Graph().as_default():
            with self.facenet_session.as_default():
                # Run forward pass to calculate embeddings
                feed_dict = { self.images_placeholder: faces, self.phase_train_placeholder:False }
                emb_array = self.facenet_session.run(self.embeddings, feed_dict = feed_dict)
        return emb_array

    def detect_target_face(self, img):
        """detect target face

        # Arguments:
            img: ndarray, the input image
                recommend load img by: misc.imread(img_path, mode='RGB')
        # Returns:
            sign: the identification of the detect result
                -1 means no face detected
                0  means common people(no target face detected)
                >0 means hit target face, positive number correspond to the index of target person
        """
        if not isinstance(img, np.ndarray):
            raise RuntimeError("format error! input parameter img is not a numpy.ndarray")
        if len(img.shape) != 3:
            raise RuntimeError("shape error! input parameter img's shape is illegal")

        # detect face and align face
        face_array = self.detect_face_and_align(img)

        # can not find a face
        if len(face_array) < 1:
            return -1

        # extract face embedding
        emb_array = self.extract_face_embedding(face_array)

        # compute distance between face embeddings
        for emb in emb_array:
            for idx, template_emb in enumerate(self.template_face_embeddings):
                dist = self._compute_distance(template_emb, emb)
                if dist < self.distance_threshold:
                    return idx + 1  # return the template idx(count from 1)
        return 0 # correspond to the situation: find face but miss hit


'''
Installation of NumPy

* If you have Python and PIP already installed on a system, then installation of NumPy is very easy.

* Install it using this command:
    $ pip install numpy

If this command fails, then use a python distribution that already has NumPy installed like, Anaconda, Spyder etc.

Import NumPy

* Once NumPy is installed, import it in your applications by adding the import keyword:

    import numpy

Now NumPy is imported and ready to use.

NumPy as np

* NumPy is usually imported under the np alias.
* alias: In Python alias are an alternate name for referring to the same thing.

Checking NumPy Version

* The version string is stored under __version__ attribute.

'''
import numpy as np

print('Checking NumPy Version:', np.__version__)


from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

import argparse
import glob
import numpy as np
import cv2
import os
import time

from tensorflow.lite.python.interpreter import Interpreter

from modules.utils import (set_memory_growth, load_yaml, get_bbox_landm, draw_bbox_landm,
                           pad_input_image, recover_pad_output)
from modules.utils import preprocess_input, prior_box, decode, non_max_suppression, postprocess

# メインの実行
def inference():
    target_dir = 'samples'
    det_model_file = os.path.join("Models", "converted_savedmodel.tflite")
    land_model_file = os.path.join("Models", "./wflw_moruhard_grow_68_Res50.tflite")
    cfg_path = './configs/retinaface_res50.yaml'
    iou_th = 0.4
    score_th = 0.5
    cfg = load_yaml(cfg_path)

    ####### retinaface interpreter
    # インタプリタの生成
    det_interpreter = Interpreter(model_path=det_model_file)
    det_interpreter.allocate_tensors()
    # 入力情報と出力情報の取得
    det_input_details = det_interpreter.get_input_details()
    det_output_details = det_interpreter.get_output_details()
    # 幅と高さの取得
    # import pdb; pdb.set_trace()
    det_width  = det_input_details[0]['shape'][1]
    det_height = det_input_details[0]['shape'][2]

    ######### resnest interpreter
    # インタプリタの生成
    land_interpreter = Interpreter(model_path=land_model_file)
    land_interpreter.allocate_tensors()
    # 入力情報と出力情報の取得
    land_input_details = land_interpreter.get_input_details()
    land_output_details = land_interpreter.get_output_details()
    # 幅と高さの取得
    image_size  = land_input_details[0]['shape'][1]

    ######### process img
    print("[*] Processing on images {}".format(target_dir))
    img_paths = glob.glob(target_dir + '/*')
    for img_path in img_paths:
        target = cv2.imread(img_path)
        # 入力画像前処理
        img_height_raw, img_width_raw, img, pad_params = preprocess_input(target, cfg)

        # 入力データの生成
        det_input_data = np.expand_dims(img, axis=0)
        det_interpreter.set_tensor(det_input_details[0]['index'], det_input_data)

        # 推論の実行
        st = time.time()
        det_interpreter.invoke()
        print("bb el: ", time.time() - st)

        det_output_data = [
            det_interpreter.get_tensor(det_output_details[1]['index']),
            det_interpreter.get_tensor(det_output_details[2]['index']),
            det_interpreter.get_tensor(det_output_details[0]['index'])
        ]

        outputs = postprocess(det_output_data, cfg, iou_th, score_th)
        # print("outputs: ", outputs.shape)

        # recover padding effect
        outputs = recover_pad_output(outputs, pad_params)

        for prior_index in range(len(outputs)):
            box, _ = get_bbox_landm(
                outputs[prior_index], img_height_raw, img_width_raw)
            x1, y1, x2, y2 = (np.array(box[:4])+0.5).astype(np.int32)
            w = x2 - x1 + 1
            h = y2 - y1 + 1

            size = int(max([w, h])*1.1)
            cx = x1 + w//2
            cy = y1 + h//2
            x1 = cx - size//2
            x2 = x1 + size
            y1 = cy - size//2
            y2 = y1 + size

            dx = max(0, -x1)
            dy = max(0, -y1)
            x1 = max(0, x1)
            y1 = max(0, y1)

            edx = max(0, x2 - img_width_raw)
            edy = max(0, y2 - img_height_raw)
            x2 = min(img_width_raw, x2)
            y2 = min(img_height_raw, y2)
            cropped = target[y1:y2, x1:x2]

            if dx > 0 or dy > 0 or edx > 0 or edy > 0:
                cropped = cv2.copyMakeBorder(cropped, dy, edy, dx, edx, cv2.BORDER_CONSTANT, 0)
            # cv2.imwrite('croped.jpg', cropped)
            # 輪郭点モデルの入力データの生成
            input = cv2.resize(cropped, (image_size, image_size))
            input_data = cv2.cvtColor(input, cv2.COLOR_BGR2RGB)
            input_data = input_data.astype(np.float32) / 256
            input_data = np.expand_dims(input_data, 0)
            print(input_data.shape)
            land_interpreter.set_tensor(land_input_details[0]['index'], input_data)

            # 輪郭点推論の実行
            st = time.time()
            land_interpreter.invoke()
            print("land el: ", time.time() - st)

            output_data = land_interpreter.get_tensor(land_output_details[-1]['index'])
            pre_landmark = output_data.reshape(-1, 2) * [image_size, image_size]

            # visualization
            cv2.rectangle(target, (x1, y1), (x2, y2), (0, 255, 0))
            pre_landmark = pre_landmark * [size/image_size, size/image_size] - [dx, dy]
            for (x, y) in pre_landmark.astype(np.int32):
                cv2.circle(target, (x1 + x, y1 + y), 2, (0, 0, 255), 2)
        save_img_path = os.path.join('results', 'out_' + os.path.basename(img_path))
        cv2.imwrite(save_img_path, target)
        print(f"[*] save result at {save_img_path}")

if __name__ == '__main__':
    inference()


from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import (QPushButton, QWidget, QLineEdit, QApplication, QMainWindow, QFileDialog, QTableWidget, QTableWidgetItem, QListWidgetItem, QListWidget, QInputDialog, QAbstractItemView, QMessageBox, QFileSystemModel, QTreeView)
from PyQt5.QtCore import QDir, Qt
from PyQt5.QtGui import QPixmap
from resource_rc import *
import sys
from newUI import Ui_MainWindow
from cellDialog import Ui_cellDialog
from cellDia2 import Ui_cellDialog2
from pathDia import Ui_Dialog
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar
import matplotlib.pyplot as plt
import geopandas as gpd
import os
from matplotlib import cm as cmaps
# import earthpy as et
import random
from gis.template import *
from gis.split import *
from gis.test import *
from gis.algo import *
from gis.shapefile_to_geojson import *
from gis.pos import *
import numpy as np
import fiona
import heapq
import matplotlib.image as mpimg
import os.path
from shapely.geometry import Polygon
from shapely.geometry import Point
import shapely
class MyForm(QMainWindow):

    # VARIABLES

    __text = int()
    def __init__(self, parent=None):
        QWidget.__init__(self, parent)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.openButton.clicked.connect(self.getFile)
        self.ui.saveButton.clicked.connect(self.savefile)
        self.ui.saveasButton.clicked.connect(self.saveasfile)
        self.ui.menuCreatePath.triggered.connect(self.showChooseDialog)
        # self.ui.menuSplit.triggered.connect(self.create_grid)
        self.setAcceptDrops(True)
        # self.setDragDropMode(QAbstractItemView.InternalMove)
        # self.ui.setAcceptDrops(True)
        self.ui.frame_3.setAcceptDrops(True)
        self.cd = Ui_cellDialog(self)
        self.cd2 = Ui_cellDialog2(self)
        self.pd = Ui_Dialog(self)
        # print(self.cd.startCell.text())
        # print(self.cd.endCell.text())
        self.cd.okButtonCd.clicked.connect(self.get_position)
        self.cd.cancelButtonCd.clicked.connect(self.cd.close)
        self.pd.pushButton.clicked.connect(self.showCellDialog)
        self.pd.pushButton_2.clicked.connect(self.showCellDialog2)

        # self.ui.setDragDropMode(QAbstractItemView.InternalMove)

    def get_position(self):
        pos1 = self.cd.startCell.text()
        pos2 = self.cd.endCell.text()
        print(pos1, pos2)
        arr_pos1 = get_coordinates(pos1)
        if arr_pos1 is None:
            raise ValueError("Enter a legit start position")
        arr_pos2 = get_coordinates(pos2)
        if arr_pos2 is None:
            raise ValueError("Enter a legit start position")
        print(arr_pos1)
        print(arr_pos2)
        self.create_grid(get_dercs(pos1))
        start = self.get_cell(arr_pos1[0], arr_pos1[1])
        goal = self.get_cell(arr_pos2[0], arr_pos2[1])
        # print(start)
        # print(goal)

        # PROCESS FOR GET DATA FROM out2.json and and calulate the path
        print("calculating path")
        # start = (30,71)
        # goal = (0,0)
        global board
        grid = np.array(board)
        board = [1]*4363 + 2000*[0]
        random.shuffle(board)
        board = [board[i:i+7] for i in range(0, 100, 10)]
        print(board)
        route = astar(grid, start, goal)
        route = route + [start]
        route = route[::-1]
        print(route)
        self.get_coords(route)
        self.cd.close()
        return True

    def get_coords(self, route):
        pnt_coords = []
        mid_pnt = length / 2
        for row in route:
            for j in range(2):
                # print(row[j], end = " ")
                if(j == 0):
                    row_coords = (ymax - mid_pnt) - row[j]*length
                    print(row_coords, end = ' ')
                    pnt_coords.append(row_coords)
                elif(j == 1):
                    row_coords = (xmin + mid_pnt) + row[j]*length
                    print(row_coords)
                    pnt_coords.append(row_coords)
                # new =
            route_points.append(list(reversed(pnt_coords)))
            pnt_coords = []
        self.create_line_shp()
            # print()

    def create_line_shp(self):
        # print(route_points)
        # line_string = Point(route_points)
        line_string = []
        for row in route_points:
            # print(row)
            line_string.append(Point(row))
        path = gpd.GeoDataFrame({'geometry':line_string})
        path.crs = {'init' :'epsg:4326'}
        path.to_file('./path.shp')
        list_of_shp_files.append('./path.shp')
        self.c_plot()
        self.get_table()

     # INIT BEAT FILE SPLIT
    def split_beat(self):
        try:
            if list_of_shp_files is None:
                raise ValueError("Please upload a file")
            else:
                self.f_name = list_of_shp_files[len(list_of_shp_files)-1]
                if "BEAT.shp" in self.f_name:
                    self.beat = SplitBeatFile(self.f_name)
                    self.beat.cal_area()
                    self.beat.split()
                    print("Split done right")
                    ok = QMessageBox.about(self, ' ', "Beats Successfully Split")
                    # ok.setIcon(QMessageBox.Information)
                    # msg.setText("Beats Successfully Split!")
                    # msg.show()
                else:
                    raise ValueError("No BEAT file")

        except ValueError as e:
            print(e)
            ok = QMessageBox.about(self, ' ', "No BEAT file")
        except:
            print("Something went wrong, Try uploading a BEAT file")
            ok = QMessageBox.about(self, "Something Went Wrong", "Try uploading a BEAT file")

    def display_beat(self):
        try:
            matchers = ["BEAT.shp", "Wireless_station.shp"]
            matching = [s for s in list_of_shp_files if any(xs in s for xs in matchers)]
            if matching is None or len(matching)==0:
                raise ValueError("Please upload 'BEAT.shp' and 'Wireless_station.shp'.")
            else:
                file_json = SaveToGeoJSON()
                for i in matching:
                    file_save_name = file_json.file_name()
                    file_json.save(i, file_save_name)
                print("files saved")
                merge_obj = MergeGeoJSON()
                merge_obj.save()
                print("File merged")
                # ok = QMessageBox.about(self, ' ', "File saved")
        except ValueError as e:
            print(e)

    def create_grid(self, descr):
        # point_shp = gpd.read_file('../shapefiles/Wireless_station.shp')
        # print(f_name)
        print(descr)
        polygon_shp = gpd.read_file('../shapefiles/BEAT.shp')
        polygon_shp['geometry'] = polygon_shp['geometry'].to_crs(epsg=4326)
        # print(poly['geometry'][0].coords.xy)
        # print(poly.geometry.centroid.coords[0].x)
        for i in range(0, len(polygon_shp['geometry'])):
            if polygon_shp['BEAT_N'][i] == descr:
                polygon = polygon_shp['geometry'][i]
        poly = shapely.geometry.asShape(polygon)
        global xmin,ymin,xmax,ymax, rows, cols
        xmin,ymin,xmax,ymax = poly.bounds
        print(xmin,ymin,xmax,ymax,length,width)
        rows = int(np.ceil((ymax-ymin) /  length))
        cols = int(np.ceil((xmax-xmin) / width))
        print(rows, cols)
        XleftOrigin = xmin
        XrightOrigin = xmin + width
        YtopOrigin = ymax
        YbottomOrigin = ymax- length
        polygons = []
        # new = []
        print(type(grid_mat))
        for i in range(cols):
            Ytop = YtopOrigin
            Ybottom =YbottomOrigin
            for j in range(rows):
                polygons.append(Polygon([(XleftOrigin, Ytop), (XrightOrigin, Ytop), (XrightOrigin, Ybottom), (XleftOrigin, Ybottom)]))
                Ytop = Ytop - length
                Ybottom = Ybottom - length
                # new.append(0)
                grid_mat[j][i] = 0
            XleftOrigin = XleftOrigin + width
            XrightOrigin = XrightOrigin + width
            # grid_mat.append(new)

        print(len(grid_mat))
        print(len(grid_mat[0]))
        print(rows, cols)
        f_data = open("new_data.txt", "w+")

        f_data.write(str(grid_mat))
        f_data.close()

        grid = gpd.GeoDataFrame({'geometry':polygons})
        # grid.to_file('grid.shp')
        # grid['geometry'] = grid['geometry'].to_crs(epsg=32644)
        grid.crs = {'init' :'epsg:4326'}
        grid.to_file('./grid.shp')
        print("here")
        # print(self.get_cell(80.6174166666667, 22.3383333333333))
        # print(grid)
        list_of_shp_files.append('./grid.shp')
        self.c_plot()
        self.get_table()
        # list_of_shp_files.append('./grid.shp')
        # self.c_plot()
        # self.get_table()
        ok = QMessageBox.about(self, ' ', "Process Started")

    def get_cell(self, g_lat, g_long):
        print(g_long, ymin)
        x_pos = int(np.ceil((g_long-ymin) /  length))
        y_pos = int(np.ceil((g_lat-xmin) / width))
        print(y_pos)
        print(x_pos)
        return (rows - x_pos), y_pos

    def c_plot(self):
        shp_attributes.clear()
        random_col()
        self.ui.figure.clear()
        ax = self.ui.figure.add_subplot(111)
        ax.axis('off')
        print(list_of_shp_files)
        print(list_of_shp_files[:-1])
        try:
            for i in range(len(list_of_shp_files)):
                # print(i)
                self.f_name = list_of_shp_files[i]
                # print(self.f_name)
                self.shape_file = gpd.read_file(self.f_name)
                # print(list_of_shp_files)
                # print(self.shape_file.STATE_N)
                # self.yaga= list()

                # PROCESSING FOR ATTRIBUTE
                prop_keys_list = []
                prop_attr_list = []
                # RUNNING BOTH LOOPS PARALLEL

                # for i in range(1000):
                #     prop_attr_list.append(i)
                #     prop_keys_list.append(i)

                shp_attributes.append(self.shape_file)
                # print(shp_attributes.geometry)
                # print(self.yaga)
                # shp_attributes.append(self.yaga)
                # print(self.shape_file)
                # print(self.yaga)
                # CHECK IF THE PROJECT OF THE SHAPEFILE IS SAME OR NOT AND THEN SET IT ACCORDINGLY

                if self.shape_file.crs['init'] != 'epsg:32644':
                    print("work")
                    # print(self.shape_file.private variables in pythoncrs['init'])
                    self.data_proj = self.shape_file.copy()
                    self.data_proj['geometry'] = self.data_proj['geometry'].to_crs(epsg=32644)
                else:
                    print("work")
                    self.data_proj = self.shape_file.copy()
                # print(self.f_name)
                if(self.f_name == './grid.shp'):
                    print("grid plot")
                    self.data_proj .plot(categorical=True,
                               ax=ax,
                               legend=True,
                               facecolor="none",
                               edgecolor='black',
                               figsize=(60, 60),
                               markersize=45
                              )
                else:
                    self.data_proj .plot(categorical=True,
                               ax=ax,
                               color=color_list[i],
                               legend=True,
                               edgecolor='black',
                               figsize=(60, 60),
                               markersize=45
                              )
                # print("lolol " + self.data_proj['geometry'][0].type)
                if(self.data_proj['geometry'][0].type == 'Point'):
                    print(self.data_proj['geometry'][0])
                    self.data_proj.apply(lambda x: ax.annotate(s='P ' + x.Name, xy=x.geometry.centroid.coords[0],
                        ha='center', va='bottom', weight='bold'),axis=1)
                    # print(self.data_proj.geometry.centroid.x.iloc[0])
                    self.geojson_file = gpd.read_file(self.f_name)
                    self.geojson_file.to_file('./out2.json', driver='GeoJSON')
                    # print(self.data_proj.Name)
                    # # print("ewsedrtfygvhbjbh")
                    # for idx, row in c.iterrows():
                    #     print(row)
                    #     self.data_proj.annotate(s='P' + (idx + 1), xy=row.geometry.centroid.coords[0],
                    #                 horizontalalignment='center')

                for feat in fiona.open(self.f_name):
                    prop_keys_list = feat.keys()

                for i in prop_keys_list:
                    for feat in fiona.open(self.f_name):
                        prop_attr_list.append(feat[i])

                for i, j in zip(prop_keys_list, prop_attr_list):
                    print(i,j)
        except:
            #print("ERROR!!!")
            del list_of_shp_files[-1]
        print("is it working")
        print(xmin,ymin,xmax,ymax)
        self.ui.canvas.draw()

    def get_path_with_grid(self):
        print(type(self.cd.startCell.text()))
        print(self.cd.endCell.text())
        self.start = self.cd.startCell.text()
        print(type(self.start))
        self.end = self.cd.endCell.text()

        print("WORKING")
        print(self.__text)
        try:
            self.cd.view1.clear()
            self.path = 'beat' + str(self.__text) + '.png'
            print(self.path)
            self.beat_path = PathGeneration(self.path)
            self.beat_path.add_grid()
            self.beat_path.cal_path(int(self.start), int(self.end))
            self.beat_path.draw_path()
            self.cd.show()
            pixmap= QPixmap('./newGrid.png')
            grap= QtWidgets.QGraphicsPixmapItem(pixmap)
            self.cd.view1.addItem(grap)
            self.cd.canvas1.show()
        except Exception as e:
            print(e)
            print("Error")
            ok = QMessageBox.about(self, ' ', "File not iterable")

    def showCellDialog(self):

        try:
            self.cd.show()

        except:
            print("Error")
            ok = QMessageBox.about(self, ' ', "File not iterable")

    def showCellDialog2(self):
        {
            self.cd2.show()
        }

    def getFile(self):

            try:
                #self.fname = ""
                self.fname = QFileDialog.getOpenFileName(self,'Open File','D:\Work\Basemaps\Basemaps', "Shape Files (*.shp) ;; Image Files (*.png,*.jpg)")
                #print(self.fname[0])
                if self.fname is not None:
                    if self.fname[0] != "":
                        list_of_shp_files.append(self.fname[0])
                        # shp_attributes.append(self.shape_file[0])
                        #print("XD")
                        self.c_plot()
                        self.get_table()
                        # self.ui.listWidget.setCurrentItem(shp_attributes)
            except:
                print(sys.exc_info()[0], "Exception Caught 1")

    def dragEnterEvent(self, event):
        if event.mimeData().hasUrls():
            event.accept()
        else:
            event.ignore()

    def dropEvent(self, event):

            # self.fname = QtCore.QUrl.fileName(event.mimeData().urls())
            for url in event.mimeData().urls():
                list_of_shp_files.append(url.toLocalFile())
            self.c_plot()
            self.get_table()

    def savefile(self):
            try:
                fname = QFileDialog.getSaveFileName(self,'Save File','d:\\',"Shape Files (*.shp)")
            except:
                print(sys.exc_info()[0], "Excpetion Caught")

    def saveasfile(self):
            try:
                fname = QFileDialog.getSaveFileName(self,'Save As','d:\\')
            except:
                print(sys.exc_info()[0], "Excpetion Caught")

    def get_table(self):

            self.ui.listWidget.clear()
            self.ui.listWidget.addItems(list_of_shp_files)

    def showDialog(self):

            self.__text, ok = QInputDialog.getText(self, 'Enter Beat Region',
                'Beat Region Number:')

            if ok:
                self.showChooseDialog()

    def showChooseDialog(self):
            self.pd.show()

if __name__ == '__main__':
    app = QApplication(sys.argv)
    mainwin = MyForm()
    mainwin.show()
    sys.exit(app.exec_())


import pysam
from math import ceil
import pandas as pd
import numpy as np
from collections import Counter
from scipy.stats import entropy

class Phaser:
    def __init__(self,splitter,sampleName,
                 aggressive=False,log=None):
        self.sampleName = sampleName   #sample name
        self.aggressive = aggressive   #aggressively separate groups
        self.splitter   = splitter     #split generator
        self.log        = log
        #################
        self.vTree      = None
        self.pending    = []

    def __repr__(self):
        if self.vTree:
            return str(self.vTree)
        else:
            return f'Phaser: {self.sampleName}'

    def run(self):
        #initialize starting conditions
        if self.log:
            self.log.info('Initializing Tree')
        self._initGroups()
        #split groups
        if self.log:
            self.log.info('Splitting Groups')
        self._splitGroups()
        # identify residual node and dump in noise bin
        if self.log:
            self.log.info('Cleaning Up')
        #self._dumpResidual()
        #get clusters
        self._getClusters()

    def _initGroups(self):
        '''start splitting tree'''
        #init labeler
        self.labeler = self.indexer()
        #all reads
        rootLbl = next(self.labeler)
        rootGrp = self.splitter.readnames
        root = vCluster(rootGrp,
                        rootLbl,
                        split=self.sampleName,
                        pending=-True)
        #group tree
        if self.log:
            self.log.debug(f'Starting vTree with {root}. Minimum reads: {self.splitter.minCount}')
        self.vTree = vTree(root)
        #reads matching reference at all selected pos
        #refcall = self.sigVar.index[(self.sigVar == '.').all(axis=1)]
        self._updatePending()
        return

    def _updatePending(self):
        self.pending = sorted([node.label
                               for node in self.vTree.nodes.values()
                               if node.pending],
                              reverse=True)
        if self.log:
            self.log.debug(f'Pending nodes: {self.pending}')

    def _splitGroups(self):
        #func to det if big enough to split
        if self.aggressive:
            canSplit = (lambda t: len(t) > self.splitter.minCount)
        else:
            canSplit = (lambda t: len(t) >= 2*self.splitter.minCount)
        while len(self.pending):
            label   = self.pending.pop()
            self.vTree[label].pending = False
            testSet = set(self.vTree[label].reads)
            subset,pos,vnt = self.splitter.split(testSet)
            if self.log and subset is not None:
                self.log.debug(f'{self.vTree[label]} split: {len(subset)},{pos},{vnt}')
            if subset is None:
                continue
                #self.vTree[label].pending = False
            else:
                #joined by variant
                newGroup = vCluster(subset,
                                    next(self.labeler),
                                    parent =label,
                                    split  =(pos,vnt),
                                    pending=canSplit(subset))
                self.vTree.append(newGroup)
                #leftovers
                complement = testSet.difference(subset)
                remainder  = vCluster(complement,
                                      next(self.labeler),
                                      parent =label,
                                      split  =(pos,'.'),
                                      pending=canSplit(complement))
                self.vTree.append(remainder)
            self._updatePending()
        return

    def _isRefCall(self,lbl):
        plrty = self.splitter.sigVar.reindex(self.vTree[lbl].reads)\
                             .apply(pd.Series.value_counts).fillna(0).idxmax()
        return (plrty.astype(str)== '.').all()

    def _getClusters(self):
        '''map of node label -> cluster'''
        #check for refcall -- all '.' variants for alignment method.  passes through for dbg meth
        offset = 0
        self.node2cluster = {}
        isRefcall = self._isRefCall if hasattr(self.splitter,'refFasta') else (lambda lbl: False)
        for lbl in self.vTree.leaves:
            leaf = self.vTree[lbl]
            if len(leaf) < self.splitter.minCount:
                #dump in noise bin
                if self.log:
                    self.log.debug(f'{leaf} has fewer than {self.splitter.minCount} reads. Labelled as "noise"(-1)')
                self.vTree.noise.update(leaf.reads)
                leaf.setNoise()
            elif isRefcall(lbl):
                #set as first
                leaf.setRefCall()
                self.node2cluster[lbl] = offset
                offset += 1
        #check if refcall was found, else increase offset if not found but there is one
        if offset == 0 and hasattr(self.splitter,'refFasta') and not self.splitter.refFasta is None:
            offset += 1
        #clusters sorted by size, descending
        sortedLeaves = sorted(filter(lambda n: not (self.vTree[n]._isNoise or self.vTree[n]._isRefCall),
                                     self.vTree.leaves),
                              key=lambda n:-len(self.vTree[n]))
        self.node2cluster.update({node : idx + offset
                                  for idx,node in enumerate(sortedLeaves)})
        #update nodes
        for node,clust in self.node2cluster.items():
            self.vTree[node].set_cluster(clust)
        return

    def _getPlurality(self,vTable):
        return vTable.apply(pd.Series.value_counts)\
                     .fillna(0).idxmax()

    def summary(self):
        '''return summary of results in dataframe format'''
        #most common call by cluster/group
        plurality = self.sigVar.groupby(self.clusterMap).apply(self._getPlurality)
        #remove columns with all refcall (.) or del-continue (*)
        varCols   = plurality.columns[~plurality.isin(['.','*']).all(axis=0)]
        #new table
        summary              = plurality[varCols].copy()
        countMap             = Counter(self.clusterMap.values())
        summary['nReads']    = summary.index.map(countMap)
        summary.index        = np.where(summary.index >= 0,
                                        'cluster' + summary.index.astype(str),
                                        'ResidualNoise')
        summary.index.name   = 'Group'
        summary.columns.name = 'RefPos'
        return summary

    @property
    def clusterMap(self):
        '''dict of {readname:cluster}'''
        clustMap = {name:-1 for name in self.vTree.noise}
        for node,clust in self.node2cluster.items():
            clustMap.update({name:clust for name in self.vTree[node].reads})
        return clustMap

    def indexer(self,start=0):
        i=start
        while True:
            yield i
            i+=1

class vCluster:
    def __init__(self,reads,label,parent=None,split=None,pending=True):
        self.label    = label
        self.reads    = reads
        self.parent   = parent
        self.split    = split
        self.children = []
        self.pending  = pending
        #private
        self._isLeaf    = False
        self._cluster   = None
        self._isNoise   = False
        self._isRefCall = False
    def set_cluster(self,cluster):
        self._isLeaf  = True
        self._cluster = cluster
    def setNoise(self):
        self._isNoise = True
    def setRefCall(self):
        self._isRefCall = True
        self._cluster   = 0
    def clusterName(self):
        assert self._cluster is not None
        return f'cluster{self._cluster}_numreads{len(self.reads)}'
    def __len__(self):
        return len(self.reads)
    def __repr__(self):
        outstr = f'Node {self.label} ({len(self.reads)} reads)'
        if self._isNoise:
            outstr += ' Noise'
        elif self._isRefCall:
            outstr += f' Cluster {self._cluster} (refcall)'
        elif self._isLeaf and self._cluster is not None:
            outstr += f' Cluster {self._cluster}'
        return outstr

class vTree:
    def __init__(self,root):
        if not root.parent is None:
            raise Phaser_Error('root parent must = None')
        self.root   = root
        self.label  = root.label
        self.size   = len(root)
        self.leaves = [root.label]
        self.nodes  = {root.label:root}
        self.noise  = set()
    def append(self,other):
        if other.parent is None or other.parent not in self.nodes:
            raise Phaser_Error('invalid parent')
        if other.split is None:
            raise Phaser_Error('please set split value')
        self.nodes[other.label] = other
        parent = self.nodes[other.parent]
        if len(parent.children) == 0:
            #remove parent as leaf
            self.leaves.pop(self.leaves.index(parent.label))
        #add this leaf
        self.leaves.append(other.label)
        parent.children.append(other.label)
    def getSplits(self,label):
        if label not in self.nodes:
            raise Phaser_Error('invalid label')
        splits = []
        leaf   = label
        while leaf:
            vclust = self.nodes[leaf]
            splits.insert(0,vclust.split)
            leaf = vclust.parent
        return splits
    def isRefcall(self,label):
        '''identify refcall group -- reads that did not split out by any variants'''
        spl = self.getSplits(label)
        if len(spl) == 0:
            return False
        else:
            return np.all([s=='.' for p,s in spl]) and len(self[label].children)==0
    def _splitStr(self,label):
        '''join splits for output'''
        splits = self.getSplits(label)
        splits.insert(0,'all')
        return ' -> '.join(map(str,splits))
    def __getitem__(self,node):
        return self.nodes[node]
    def __iter__(self):
        for lbl,node in self.nodes.items():
            yield node
    def __repr__(self):
        leaves = [f'{str(self[lbl])} : {self._splitStr(lbl)}' for lbl in sorted(self.leaves)]
        return '\n'.join(leaves)

class Phaser_Error(Exception):
    pass


import math
import threading
import time
from collections import namedtuple, defaultdict
from enum import Enum, IntEnum, auto
from functools import partial
from typing import Union, Callable, Dict, Iterable

import numpy as np
from transforms3d.euler import euler2mat

from spherov2.commands.animatronic import R2LegActions
from spherov2.commands.io import IO
from spherov2.commands.power import BatteryVoltageAndStateStates
from spherov2.controls import RawMotorModes
from spherov2.helper import bound_value, bound_color
from spherov2.toy import Toy
from spherov2.toy.bb8 import BB8
from spherov2.toy.bb9e import BB9E
from spherov2.toy.bolt import BOLT
from spherov2.toy.mini import Mini
from spherov2.toy.ollie import Ollie
from spherov2.toy.r2d2 import R2D2
from spherov2.toy.r2q5 import R2Q5
from spherov2.toy.rvr import RVR
from spherov2.toy.sphero import Sphero
from spherov2.types import Color
from spherov2.utils import ToyUtil

class Stance(str, Enum):
    Bipod = 'twolegs'
    Tripod = 'threelegs'

class EventType(Enum):
    on_collision = auto()  # [f.Sphero, f.Ollie, f.BB8, f.BB9E, f.R2D2, f.R2Q5, f.BOLT, f.Mini]
    on_freefall = auto()  # [f.Sphero, f.Ollie, f.BB8, f.BB9E, f.R2D2, f.R2Q5, f.BOLT, f.Mini]
    on_landing = auto()  # [f.Sphero, f.Ollie, f.BB8, f.BB9E, f.R2D2, f.R2Q5, f.BOLT, f.Mini]
    on_gyro_max = auto()  # [f.Sphero, f.Mini, f.Ollie, f.BB8, f.BB9E, f.BOLT, f.Mini]
    on_charging = auto()  # [f.Sphero, f.Ollie, f.BB8, f.BB9E, f.R2D2, f.R2Q5, f.BOLT]
    on_not_charging = auto()  # [f.Sphero, f.Ollie, f.BB8, f.BB9E, f.R2D2, f.R2Q5, f.BOLT]
    on_magnetometer_north_yaw = auto()  # [f.BOLT] TODO
    on_sensor_streaming_data = auto()  # [f.BOLT] TODO
    on_ir_message = auto()  # [f.BOLT, f.RVR] TODO
    on_color = auto()  # [f.RVR] TODO

class LedManager:
    def __init__(self, cls):
        if cls is RVR:
            self.__mapping = {
                'front': ('left_headlight', 'right_headlight'),
                'main': ('left', 'right', 'front', 'back')
            }
        elif cls in (R2D2, R2Q5, BOLT):
            self.__mapping = {'main': ('front', 'back')}
        else:
            self.__mapping = {}
        self.__leds = defaultdict(partial(Color, 0, 0, 0))

    def __setitem__(self, key, value):
        if key in self.__mapping:
            for led in self.__mapping[key]:
                self.__setitem__(led, value)
        else:
            self.__leds[key] = value

    def __getitem__(self, item):
        if item in self.__mapping:
            return self.__getitem__(self.__mapping[item][0])
        return self.__leds[item]

    def get(self, item, default):
        if item in self.__mapping:
            return self.get(self.__mapping[item][0], default)
        return self.__leds.get(item, default)

rawMotor = namedtuple('rawMotor', ('left', 'right'))

class SpheroEduAPI:
    """Implementation of Sphero Edu Javascript APIs: https://sphero.docsapp.io/docs/get-started"""

    def __init__(self, toy: Toy):
        self.__toy = toy
        self.__heading = 0
        self.__speed = 0
        self.__stabilization = True
        self.__raw_motor = rawMotor(0, 0)
        self.__leds = LedManager(toy.__class__)

        self.__sensor_data: Dict[str, Union[float, Dict[str, float]]] = {'distance': 0., 'color_index': -1}
        self.__sensor_name_mapping = {}
        self.__last_location = (0., 0.)
        self.__last_non_fall = time.time()
        self.__falling_v = 1.
        self.__last_message = None
        self.__should_land = self.__free_falling = False

        self.__listeners = defaultdict(set)
        ToyUtil.add_listeners(toy, self)

        self.__stopped = threading.Event()
        self.__stopped.set()
        self.__updating = threading.Lock()
        self.__thread = None

    def __enter__(self):
        self.__stopped.clear()
        self.__thread = threading.Thread(target=self.__background)
        self.__toy.__enter__()
        self.__thread.start()
        try:
            self.__toy.wake()
            ToyUtil.set_robot_state_on_start(self.__toy)
            self.__start_capturing_sensor_data()
        except:
            self.__exit__(None, None, None)
            raise
        return self

    def __exit__(self, *args):
        self.__stopped.set()
        self.__thread.join()
        try:
            ToyUtil.sleep(self.__toy)
        except:
            pass
        self.__toy.__exit__(*args)

    def __background(self):
        while not self.__stopped.wait(0.8):
            with self.__updating:
                self.__update_speeds()

    def _will_sleep_notify(self):
        ToyUtil.ping(self.__toy)

    # Movements: control the robot's motors and control system.
    def __update_speeds(self):
        if self.__speed != 0:
            self.__update_speed()
        if self.__raw_motor.left != 0 or self.__raw_motor.right != 0:
            self.__update_raw_motor()

    def __stop_all(self):
        if self.__speed != 0:
            self.__speed = 0
            self.__update_speed()
        if self.__raw_motor.left != 0 or self.__raw_motor.right != 0:
            self.__raw_motor = rawMotor(0, 0)
            self.__update_raw_motor()

    def roll(self, heading: int, speed: int, duration: float):
        """Combines heading(0-360°), speed(-255-255), and duration to make the robot roll with one line of code.
        For example, to have the robot roll at 90°, at speed 200 for 2s, use ``roll(90, 200, 2)``"""
        if isinstance(self.__toy, Mini) and speed != 0:
            speed = round((speed + 126) * 2 / 3) if speed > 0 else round((speed - 126) * 2 / 3)
        self.__speed = bound_value(-255, speed, 255)
        self.__heading = heading % 360
        if speed < 0:
            self.__heading = (self.__heading + 180) % 360
        self.__update_speed()
        time.sleep(duration)
        self.stop_roll()

    def __update_speed(self):
        ToyUtil.roll_start(self.__toy, self.__heading, self.__speed)

    def set_speed(self, speed: int):
        """Sets the speed of the robot from -255 to 255, where positive speed is forward, negative speed is backward,
        and 0 is stopped. Each robot type translates this value differently into a real world speed;
        Ollie is almost three times faster than Sphero. For example, use ``set_speed(188)`` to set the speed to 188
        which persists until you set a different speed. You can also read the real-time velocity value in centimeters
        per second reported by the motor encoders.
        """
        if isinstance(self.__toy, Mini) and speed != 0:
            speed = round((speed + 126) * 2 / 3) if speed > 0 else round((speed - 126) * 2 / 3)
        self.__speed = bound_value(-255, speed, 255)
        self.__update_speed()

    def stop_roll(self, heading: int = None):
        """Sets the speed to zero to stop the robot, effectively the same as the ``set_speed(0)`` command."""
        if heading is not None:
            self.__heading = heading % 360
        self.__speed = 0
        ToyUtil.roll_stop(self.__toy, self.__heading, False)

    def set_heading(self, heading: int):
        """Sets the direction the robot rolls.
        Assuming you aim the robot with the blue tail light facing you, then 0° is forward, 90° is right,
        270° is left, and 180° is backward. For example, use ``set_heading(90)`` to face right."""
        self.__heading = heading % 360
        ToyUtil.roll_start(self.__toy, self.__heading, self.__speed)

    def spin(self, angle: int, duration: float):
        """Spins the robot for a given number of degrees over time, with 360° being a single revolution.
        For example, to spin the robot 360° over 1s, use: ``spin(360, 1)``.
        Use :func:`set_speed` prior to :func:`spin` to have the robot move in circle or an arc or circle.

        Note: Unlike official API, performance of spin is guaranteed, but may be longer than the specified duration."""

        if angle == 0:
            return

        time_pre_rev = .45

        if isinstance(self.__toy, RVR):
            time_pre_rev = 1.5
        elif isinstance(self.__toy, (R2D2, R2Q5)):
            time_pre_rev = .7
        elif isinstance(self.__toy, Mini):
            time_pre_rev = .5
        elif isinstance(self.__toy, Ollie):
            time_pre_rev = .6

        abs_angle = abs(angle)
        duration = max(duration, time_pre_rev * abs_angle / 360)

        start = time.time()
        angle_gone = 0
        with self.__updating:
            while angle_gone < abs_angle:
                delta = round(min((time.time() - start) / duration, 1.) * abs_angle) - angle_gone
                self.set_heading(self.__heading + delta if angle > 0 else self.__heading - delta)
                angle_gone += delta

    def set_stabilization(self, stabilize: bool):
        """Turns the stabilization system on and ``set_stabilization(false)`` turns it off.
        Stabilization is normally on to keep the robot upright using the Inertial Measurement Unit (IMU),
        a combination of readings from the Accelerometer (directional acceleration), Gyroscope (rotation speed),
        and Encoders (location and distance). When ``set_stabilization(false)`` and you power the motors,
        the robot will not balance, resulting in possible unstable behaviors like wobbly driving,
        or even jumping if you set the power very high. Some use cases to turn it off are:

        1. Jumping: Set Motor Power to max values and the robot will jump off the ground!
        2. Gyro: Programs like the Spinning Top where you want to to isolate the Gyroscope readings rather than having
           the robot auto balance inside the shell.

        When stabilization is off you can't use :func:`set_speed` to set a speed because it requires the control system
        to be on to function. However, you can control the motors using Motor Power with :func:`raw_motor` when
        the control system is off."""
        self.__stabilization = stabilize
        if isinstance(self.__toy, (Sphero, Mini, Ollie, BB8, BB9E, BOLT)):
            ToyUtil.set_stabilization(self.__toy, stabilize)

    def __update_raw_motor(self):
        ToyUtil.set_raw_motor(self.__toy,
                              RawMotorModes.REVERSE if self.__raw_motor.left < 0 else RawMotorModes.FORWARD,
                              abs(self.__raw_motor.left),
                              RawMotorModes.REVERSE if self.__raw_motor.right < 0 else RawMotorModes.FORWARD,
                              abs(self.__raw_motor.right))

    def raw_motor(self, left: int, right: int, duration: float):
        """Controls the electrical power sent to the left and right motors independently, on a scale from -255 to 255
        where positive is forward, negative is backward, and 0 is stopped. If you set both motors to full power
        the robot will jump because stabilization (use of the IMU to keep the robot upright) is disabled when using
        this command. This is different from :func:`set_speed` because Raw Motor sends an "Electromotive force"
        to the motors, whereas Set Speed is a target speed measured by the encoders. For example, to set the raw motor
        to full power for 4s, making the robot jump off the ground, use ``raw_motor(255, 255, 4)``."""
        stabilize = self.__stabilization
        if stabilize:
            self.set_stabilization(False)
        self.__raw_motor = rawMotor(bound_value(-255, left, 255), bound_value(-255, right, 255))
        self.__update_raw_motor()
        if duration is not None:
            time.sleep(duration)
            if stabilize:
                self.set_stabilization(True)
            self.__raw_motor = rawMotor(0, 0)
            ToyUtil.set_raw_motor(self.__toy, RawMotorModes.OFF, 0, RawMotorModes.OFF, 0)

    def reset_aim(self):
        """Resets the heading calibration (aim) angle to use the current direction of the robot as 0°."""
        ToyUtil.reset_heading(self.__toy)

    # Star Wars Droid Movements
    def play_animation(self, animation: IntEnum):
        """Plays iconic `Star Wars Droid animations <https://edu.sphero.com/remixes/1195472/>`_ unique to BB-8, BB-9E,
        R2-D2 and R2-Q5 that combine movement, lights and sound. All animation enums can be accessed under the droid
        class, such as :class:`R2D2.Animations.CHARGER_1`."""
        if hasattr(self.__toy, 'Animations'):
            if animation not in self.__toy.Animations:
                raise ValueError(f'Animation {animation} cannot be played by this toy')
            with self.__updating:
                self.__stop_all()
            ToyUtil.play_animation(self.__toy, animation, True)

    # The R2-D2 and R2-Q5 Droids are physically different from other Sphero robots,
    # so there are some unique commands that only they can use.
    def set_dome_position(self, angle: float):
        """Rotates the dome on its axis, from -160° to 180°. For example, set to 45° using ``set_dome_position(45).``"""
        if isinstance(self.__toy, (R2D2, R2Q5)):
            ToyUtil.set_head_position(self.__toy, bound_value(-160., angle, 180.))

    def set_stance(self, stance: Stance):
        """Changes the stance between bipod and tripod. Set to bipod using ``set_stance(Stance.Bipod)`` and
        to tripod using ``set_stance(Stance.Tripod)``. Tripod is required for rolling."""
        if isinstance(self.__toy, (R2D2, R2Q5)):
            if stance == Stance.Bipod:
                ToyUtil.perform_leg_action(self.__toy, R2LegActions.TWO_LEGS)
            elif stance == Stance.Tripod:
                ToyUtil.perform_leg_action(self.__toy, R2LegActions.THREE_LEGS)
            else:
                raise ValueError(f'Stance {stance} is not supported')

    def set_waddle(self, waddle: bool):
        """Turns the waddle walk on using `set_waddle(True)`` and off using ``set_waddle(False)``."""
        if isinstance(self.__toy, (R2D2, R2Q5)):
            with self.__updating:
                self.__stop_all()
            ToyUtil.perform_leg_action(self.__toy, R2LegActions.WADDLE if waddle else R2LegActions.STOP)

    # Lights: control the color and brightness of LEDs on a robot.
    def set_main_led(self, color: Color):
        """Changes the color of the main LED light, or the full matrix on Sphero BOLT. Set this using RGB
        (red, green, blue) values on a scale of 0 - 255. For example, ``set_main_led(Color(r=90, g=255, b=90))``."""
        self.__leds['main'] = bound_color(color, self.__leds['main'])
        ToyUtil.set_main_led(self.__toy, **self.__leds['main']._asdict(), is_user_color=False)

    def set_front_led(self, color: Color):
        """For Sphero RVR: Changes the color of RVR's front two LED headlights together.

        For Sphero BOLT, R2D2, R2Q5: Changes the color of the front LED light.

        Set this using RGB (red, green, blue) values on a scale of 0 - 255. For example, the magenta color is expressed
        as ``set_front_color(Color(239, 0, 255))``."""
        if isinstance(self.__toy, (R2D2, R2Q5, BOLT, RVR)):
            self.__leds['front'] = bound_color(color, self.__leds['front'])
            ToyUtil.set_front_led(self.__toy, **self.__leds['front']._asdict())

    def set_back_led(self, color: Union[Color, int]):
        """For older Sphero:
        Sets the brightness of the back aiming LED, aka the "Tail Light." This LED is limited to blue only, with a
        brightness scale from 0 to 255. For example, use ``set_back_led(255)`` to set the back LED to full brightness.
        Use :func:`time.sleep` to set it on for a duration. For example, to create a dim and a bright blink
        sequence use::

            set_back_led(0)  # Dim
            delay(0.33)
            set_back_led(255)  # Bright
            delay(0.33)

        For Sphero BOLT, R2D2, R2Q5:
        Changes the color of the back LED light. Set this using RGB (red, green, blue) values on a scale of 0 - 255.

        For Sphero RVR:
        Changes the color of the left and right breaklight LED light. Set this using RGB (red, green, blue) values
        on a scale of 0 - 255."""
        if isinstance(color, int):
            self.__leds['back'] = Color(0, 0, bound_value(0, color, 255))
            ToyUtil.set_back_led_brightness(self.__toy, self.__leds['back'].b)
        elif isinstance(self.__toy, (R2D2, R2Q5, BOLT, RVR, Mini)):
            self.__leds['back'] = bound_color(color, self.__leds['back'])
            ToyUtil.set_back_led(self.__toy, **self.__leds['back']._asdict())

    def fade(self, from_color: Color, to_color: Color, duration: float):
        """Changes the main LED lights from one color to another over a period of seconds. For example, to fade from
        green to blue over 3s, use: ``fade(Color(0, 255, 0), Color(0, 0, 255), 3.0)``."""
        from_color = bound_color(from_color, self.__leds['main'])
        to_color = bound_color(to_color, self.__leds['main'])

        start = time.time()
        while True:
            frac = (time.time() - start) / duration
            if frac >= 1:
                break
            self.set_main_led(Color(
                r=round(from_color.r * (1 - frac) + to_color.r * frac),
                g=round(from_color.g * (1 - frac) + to_color.g * frac),
                b=round(from_color.b * (1 - frac) + to_color.b * frac)))
        self.set_main_led(to_color)

    def strobe(self, color: Color, period: float, count: int):
        """Repeatedly blinks the main LED lights. The period is the time, in seconds, the light stays on during a
        single blink; cycles is the total number of blinks. The time for a single cycle is twice the period
        (time for a blink plus the same amount of time for the light to be off). Another way to say this is the period
        is 1/2 the time it takes for a single cycle. So, to strobe red 15 times in 3 seconds, use:
        ``strobe(Color(255, 57, 66), (3 / 15) * .5, 15)``."""
        for i in range(count * 2):
            if i & 1:
                self.set_main_led(color)
            else:
                self.set_main_led(Color(0, 0, 0))
            time.sleep(period)

    # TODO Sphero BOLT Lights
    def set_matrix_pixel(self, x: int, y: int, color: Color):
        """For Sphero BOLT: Changes the color of BOLT's matrix at X and Y value. 8x8
        """
        strMapLoc: str = str(x) + ':' + str(y)
        self.__leds[strMapLoc] = bound_color(color, self.__leds[
            strMapLoc])  # TODO: Do this in a way that works with line and fill
        ToyUtil.set_matrix_pixel(self.__toy, x, y, **self.__leds[strMapLoc]._asdict(), is_user_color=False)

    def set_matrix_line(self, x1: int, y1: int, x2: int, y2: int, color: Color):
        """For Sphero BOLT: Changes the color of BOLT's matrix from x1,y1 to x2,y2 in a line. 8x8
        """
        strMapLoc: str = str(x1) + 'x' + str(y1) + '|' + str(x2) + 'x' + str(y2)
        self.__leds[strMapLoc] = bound_color(color, self.__leds[
            strMapLoc])  # TODO: Do this in a way that works with pixel and fill (needs to be accurate to diagonal lines)
        ToyUtil.set_matrix_line(self.__toy, x1, y1, x2, y2, **self.__leds[strMapLoc]._asdict(), is_user_color=False)

    def set_matrix_fill(self, x1: int, y1: int, x2: int, y2: int, color: Color):
        """For Sphero BOLT: Changes the color of BOLT's matrix from x1,y1 to x2,y2 in a box. 8x8
        """
        strMapLoc: str = str(x1) + 'x' + str(y1) + '[]' + str(x2) + 'x' + str(y2)
        self.__leds[strMapLoc] = bound_color(color, self.__leds[
            strMapLoc])  # TODO: Do this in a way that works with pixel and line
        ToyUtil.set_matrix_fill(self.__toy, x1, y1, x2, y2, **self.__leds[strMapLoc]._asdict(), is_user_color=False)

    def register_matrix_animation(self, s, s2, z, s3, s_arr, i, i_arr):  # TODO: fix this function
        ToyUtil.register_matrix_animation(self.__toy, s, s2, z, s3, s_arr, i, i_arr)

    def play_matrix_animation(self, s):
        ToyUtil.play_matrix_animation(self.__toy, s)

    # Sphero RVR Lights
    def set_left_headlight_led(self, color: Color):
        """Changes the color of the front left headlight LED on RVR. Set this using RGB (red, green, blue) values on a
        scale of 0 - 255. For example, the pink color is expressed as
        ``set_left_headlight_led(Color(253, 159, 255))``."""
        if isinstance(self.__toy, RVR):
            self.__leds['left_headlight'] = bound_color(color, self.__leds['left_headlight'])
            ToyUtil.set_left_front_led(self.__toy, **self.__leds['left_headlight']._asdict())

    def set_right_headlight_led(self, color: Color):
        """Changes the color of the front right headlight LED on RVR. Set this using RGB (red, green, blue) values on a
        scale of 0 - 255. For example, the blue color is expressed as
        ``set_right_headlight_led(0, 28, 255)``."""
        if isinstance(self.__toy, RVR):
            self.__leds['right_headlight'] = bound_color(color, self.__leds['right_headlight'])
            ToyUtil.set_right_front_led(self.__toy, **self.__leds['right_headlight']._asdict())

    def set_left_led(self, color: Color):
        """Changes the color of the LED on RVR's left side (which is the side with RVR's battery bay door). Set this
        using RGB (red, green, blue) values on a scale of 0 - 255. For example, the green color is expressed as
        ``set_left_led(Color(0, 255, 34))``."""
        if isinstance(self.__toy, RVR):
            self.__leds['left'] = bound_color(color, self.__leds['left'])
            ToyUtil.set_battery_side_led(self.__toy, **self.__leds['left']._asdict())

    def set_right_led(self, color: Color):
        """Changes the color of the LED on RVR's right side (which is the side with RVR's power button). Set this using
        RGB (red, green, blue) values on a scale of 0 - 255. For example, the red color is expressed as
        ``set_right_led(Color(255, 18, 0))``."""
        if isinstance(self.__toy, RVR):
            self.__leds['right'] = bound_color(color, self.__leds['right'])
            ToyUtil.set_power_side_led(self.__toy, **self.__leds['right']._asdict())

    # BB-9E Lights
    def set_dome_leds(self, brightness: int):
        """Controls the brightness of the two single color LEDs (red and blue) in the dome, from 0 to 15. We don't use
        0-255 for this light because it has less granular control. For example, set them to full brightness using
        ``set_dome_leds(15)``."""
        if isinstance(self.__toy, BB9E):
            self.__leds['dome'] = bound_value(0, brightness, 15)
            ranged = self.__leds['dome'] * 255 // 15
            ToyUtil.set_head_led(self.__toy, ranged)

    # R2-D2 & R2-Q5 Lights
    def set_holo_projector_led(self, brightness: int):
        """Changes the brightness of the Holographic Projector white LED, from 0 to 255. For example, set it to full
        brightness using ``set_holo_projector_led(255)``."""
        if isinstance(self.__toy, (R2D2, R2Q5)):
            self.__leds['holo_projector'] = bound_value(0, brightness, 255)
            ToyUtil.set_holo_projector(self.__toy, self.__leds['holo_projector'])

    def set_logic_display_leds(self, brightness: int):
        """Changes the brightness of the Logic Display LEDs, from 0 to 255. For example, set it to full brightness
        using ``set_logic_display_leds(255)``."""
        if isinstance(self.__toy, (R2D2, R2Q5)):
            self.__leds['logic_display'] = bound_value(0, brightness, 255)
            ToyUtil.set_logic_display(self.__toy, self.__leds['logic_display'])

    # Sounds: Control sounds and words which can play from your programming device's speaker or the robot.
    def play_sound(self, sound: IntEnum):
        """Unique Star Wars Droid Sounds are available for BB-8, BB-9E and R2-D2. For example, to play the R2-D2 Burnout
        sound use ``play_sound(R2D2.Audio.R2_BURNOUT)``."""
        if hasattr(self.__toy, 'Audio'):
            if sound not in self.__toy.Audio:
                raise ValueError(f'Sound {sound} cannot be played by this toy')
            ToyUtil.play_sound(self.__toy, sound, False)

    # Sensors: Querying sensor data allows you to react to real-time values coming from the robots' physical sensors.
    def __start_capturing_sensor_data(self):
        if isinstance(self.__toy, RVR):
            sensors = ['accelerometer', 'gyroscope', 'imu', 'locator', 'velocity', 'ambient_light', 'color_detection']
            self.__sensor_name_mapping['imu'] = 'attitude'
        elif isinstance(self.__toy, BOLT):
            sensors = ['accelerometer', 'gyroscope', 'attitude', 'locator', 'velocity', 'ambient_light']
        else:
            sensors = ['attitude', 'accelerometer', 'gyroscope', 'locator', 'velocity']
        ToyUtil.enable_sensors(self.__toy, sensors)

    def _sensor_data_listener(self, sensor_data: Dict[str, Dict[str, float]]):
        for sensor, data in sensor_data.items():
            if sensor in self.__sensor_name_mapping:
                self.__sensor_data[self.__sensor_name_mapping[sensor]] = data
            else:
                self.__sensor_data[sensor] = data
        if 'attitude' in self.__sensor_data and 'accelerometer' in self.__sensor_data:
            att = self.__sensor_data['attitude']
            r = euler2mat(*np.deg2rad((att['roll'], att['pitch'], att['yaw'])), axes='szxy')
            acc = self.__sensor_data['accelerometer']
            self.__sensor_data['vertical_accel'] = -np.matmul(np.linalg.inv(r), (acc['x'], -acc['z'], acc['y']))[1]
            self.__process_falling(self.__sensor_data['vertical_accel'])
        if 'locator' in self.__sensor_data:
            cur_loc = self.__sensor_data['locator']
            cur_loc = (cur_loc['x'], cur_loc['y'])
            self.__sensor_data['distance'] += math.hypot(cur_loc[0] - self.__last_location[0],
                                                         cur_loc[1] - self.__last_location[1])
            self.__last_location = cur_loc
        if 'color_detection' in self.__sensor_data:
            color = self.__sensor_data['color_detection']
            index = color['index']
            if index != self.__sensor_data['color_index'] and index < 255 and color['confidence'] >= 0.71:
                self.__sensor_data['color_index'] = index
                self.__call_event_listener(EventType.on_color, Color(int(color['r']), int(color['g']), int(color['b'])))

    def __process_falling(self, a):
        self.__falling_v = (self.__falling_v + a * 3) / 4
        cur = time.time()
        if -.5 < self.__falling_v < .5 if self.__stabilization else -.1 < a < .1:
            if cur - self.__last_non_fall > .2 and not self.__free_falling:
                self.__call_event_listener(EventType.on_freefall)
                self.__free_falling = True
                self.__should_land = True
        else:
            self.__last_non_fall = cur
            self.__free_falling = False
        if self.__should_land and (
                self.__falling_v < -1.1 or self.__falling_v > 1.1 if self.__stabilization else a < -.8 or a > -.8):
            self.__call_event_listener(EventType.on_landing)
            self.__should_land = False

    def _collision_detected_notify(self, args):
        self.__call_event_listener(EventType.on_collision)

    def _battery_state_changed_notify(self, state: BatteryVoltageAndStateStates):
        if state == BatteryVoltageAndStateStates.CHARGED or state == BatteryVoltageAndStateStates.CHARGING:
            self.__call_event_listener(EventType.on_charging)
        else:
            self.__call_event_listener(EventType.on_not_charging)

    def _gyro_max_notify(self, flags):
        self.__call_event_listener(EventType.on_gyro_max)

    def _magnetometer_north_yaw_notify(self, flags):
        self.__call_event_listener(EventType.on_magnetometer_north_yaw)

    def _sensor_streaming_data_notify(self, flags):
        self.__call_event_listener(EventType.on_sensor_streaming_data)

    def get_acceleration(self):
        """Provides motion acceleration data along a given axis measured by the Accelerometer, in g's, where g =
        9.80665 m/s^2.

        ``get_acceleration()['x']`` is the left-to-right acceleration, from -8 to 8 g's.

        ``get_acceleration()['y']`` is the forward-to-back acceleration, from of -8 to 8 g's.

        ``get_acceleration()['z']`` is the upward-to-downward acceleration, from -8 to 8 g's."""

        return self.__sensor_data.get('accelerometer', None)

    def get_vertical_acceleration(self):
        """This is the upward or downward acceleration regardless of the robot's orientation, from -8 to 8 g's."""
        return self.__sensor_data.get('vertical_accel', None)

    def get_orientation(self):
        """Provides the tilt angle along a given axis measured by the Gyroscope, in degrees.

        ``get_orientation()['pitch']`` is the forward or backward tilt angle, from -180° to 180°.

        ``get_orientation()['roll']`` is left or right tilt angle, from -90° to 90°.

        ``get_orientation()['yaw']`` is the spin (twist) angle, from -180° to 180°."""
        return self.__sensor_data.get('attitude', None)

    def get_gyroscope(self):
        """Provides the rate of rotation around a given axis measured by the gyroscope, from -2,000° to 2,000°
        per second.

        ``get_gyroscope().['pitch']`` is the rate of forward or backward spin, from -2,000° to 2,000° per second.

        ``get_gyroscope().['roll']`` is the rate of left or right spin, from -2,000° to 2,000° per second.

        ``get_gyroscope().['yaw']`` is the rate of sideways spin, from -2,000° to 2,000° per second."""
        return self.__sensor_data.get('gyroscope', None)

    def get_velocity(self):
        """Provides the velocity along a given axis measured by the motor encoders, in centimeters per second.

        ``get_velocity()['x']`` is the right (+) or left (-) velocity, in centimeters per second.

        ``get_velocity()['y']`` is the forward (+) or back (-) velocity, in centimeters per second."""
        return self.__sensor_data.get('velocity', None)

    def get_location(self):
        """Provides the location where the robot is in space (x,y) relative to the origin, in centimeters. This is not
        the distance traveled during the program, it is the offset from the origin (program start).

        ``get_location()['x']`` is the right (+) or left (-) distance from the origin of the program start, in
        centimeters.

        ``get_location()['y']`` is the forward (+) or backward (-) distance from the origin of the program start, in
        centimeters."""
        return self.__sensor_data.get('locator', None)

    def get_distance(self):
        """Provides the total distance traveled in the program, in centimeters."""
        return self.__sensor_data.get('distance', None)

    def get_speed(self):
        """Provides the current target speed of the robot, from -255 to 255, where positive is forward, negative is
        backward, and 0 is stopped."""
        return self.__speed

    def get_heading(self):
        """Provides the target directional angle, in degrees. Assuming you aim the robot with the tail facing you,
        then 0° heading is forward, 90° is right, 180° is backward, and 270° is left."""
        return self.__heading

    def get_main_led(self):
        """Provides the RGB color of the main LEDs, from 0 to 255 for each color channel.

        ``get_main_led().r`` is the red channel, from 0 - 255.

        ``get_main_led().g`` is the green channel, from 0 - 255.

        ``get_main_led().b`` is the blue channel, from 0 - 255."""
        return self.__leds.get('main', None)

    # Sphero BOLT Sensors
    # TODO Compass Direction

    def get_luminosity_direct(self):
        """similar to get_luminosity, however this is a more direct call to the sphero to get a value directly"""
        return ToyUtil.get_ambient_light_sensor_value(self.__toy)

    def get_luminosity(self):
        """Provides the light intensity from 0 - 100,000 lux, where 0 lux is full darkness and 30,000-100,000 lux is
        direct sunlight. You may need to adjust a condition based on luminosity in different environments as light
        intensity can vary greatly between rooms."""
        return self.__sensor_data.get('ambient_light', None)

    def get_last_ir_message(self):
        """Returns which channel the last infrared message was received on. You need to declare the ``on_ir_message``
        event for each IR message you plan to see returned."""
        return self.__last_message

    def get_back_led(self):
        """Provides the RGB color of the back LED, from 0 to 255 for each color channel."""
        return self.__leds.get('back', None)

    def get_front_led(self):
        """Provides the RGB color of the front LED, from 0 to 255 for each color channel."""
        return self.__leds.get('front', None)

    # Sphero RVR Sensors
    def get_color(self):
        """Provides the RGB color, from 0 to 255 for each color channel, that is returned from RVR's color sensor.

        ``get_color().r`` is the red channel, from 0 - 255, that is returned from RVR's color sensor.

        ``get_color().g`` is the green channel, from 0 - 255, that is returned from RVR's color sensor.

        ``get_color().b`` is the blue channel, from 0 - 255, that is returned from RVR's color sensor."""
        if 'color_detection' in self.__sensor_data:
            color = self.__sensor_data['color_detection']
            return Color(round(color['r']), round(color['g']), round(color['b']))
        return None

    # BB-9E Sensors
    def get_dome_leds(self):
        """Provides the brightness of the Dome LEDs, from 0 to 15."""
        return self.__leds.get('dome', None)

    # R2-D2 & R2-Q5 Sensors
    def get_holo_projector_led(self):
        """Provides the brightness of the Holographic Projector LED, from 0 to 255."""
        return self.__leds.get('holo_projector', None)

    def get_logic_display_leds(self):
        """Provides the brightness of the white Logic Display LEDs, from 0 to 255."""
        return self.__leds.get('logic_display', None)

    # Communications
    def start_ir_broadcast(self, near: int, far: int):
        """Sets the IR emitters to broadcast on two specified channels, from 0 to 7, so other BOLTs can follow or evade.
        The broadcaster uses two channels because the first channel emits near IR pulses (< 1 meter), and the second
        channel emits far IR pulses (1 to 3 meters) so the following and evading BOLTs can detect these messages on
        their IR receivers with a sense of relative proximity to the broadcaster. You can't use a channel for more than
        one purpose at time, such as sending messages along with broadcasting, following, or evading. For example,
        use ``start_ir_broadcast(0, 1)`` to broadcast on channels 0 and 1, so that other BOLTs following or evading on
        0 and 1 will recognize this robot."""
        ToyUtil.start_robot_to_robot_infrared_broadcasting(self.__toy, bound_value(0, far, 7), bound_value(0, near, 7))

    def stop_ir_broadcast(self):
        """Stops the broadcasting behavior."""
        ToyUtil.stop_robot_to_robot_infrared_broadcasting(self.__toy)

    def start_ir_follow(self, near: int, far: int):
        """Sets the IR receivers to look for broadcasting BOLTs on the same channel pair, from 0 to 7. Upon receiving
        messages from a broadcasting BOLT, the follower will adjust its heading and speed to follow the broadcaster.
        When a follower loses sight of a broadcaster, the follower will spin in place to search for the broadcaster.
        You can't use a channel for more than one purpose at time, such as sending messages along with broadcasting,
        following, or evading. For example, use ``start_ir_follow(0, 1)`` to follow another BOLT that is broadcasting on
        channels 0 and 1."""
        ToyUtil.start_robot_to_robot_infrared_following(self.__toy, bound_value(0, far, 7), bound_value(0, near, 7))

    def stop_ir_follow(self):
        """Stops the following behavior."""
        ToyUtil.stop_robot_to_robot_infrared_following(self.__toy)

    def start_ir_evade(self, near: int, far: int):
        """Sets the IR receivers to look for broadcasting BOLTs on the same channel pair, from 0 to 7. Upon receiving
        messages from a broadcasting BOLT, the evader will adjust its heading to roll away from the broadcaster.
        When an evader loses sight of a broadcaster, the evader will spin in place to search for the broadcaster.
        The evader may stop if it is in the far range for a period of time so it does not roll too far away from the
        broadcaster. You can't use a channel for more than one purpose at time, such as sending messages along with
        broadcasting, following, or evading. For example, use ``start_ir_evade(0, 1)`` to evade another BOLT that is
        broadcasting on channels 0 and 1."""
        ToyUtil.start_robot_to_robot_infrared_evading(self.__toy, bound_value(0, far, 7), bound_value(0, near, 7))

    def stop_ir_evade(self):
        """Stops the evading behavior."""
        ToyUtil.stop_robot_to_robot_infrared_evading(self.__toy)

    def send_ir_message(self, channel: int, intensity: int):
        """Sends a message on a given IR channel, at a set intensity, from 1 to 64. Intensity is proportional to
        proximity, where a 1 is the closest, and 64 is the farthest. For example, use ``send_ir_message(4, 5)`` to send
        message 4 at intensity 5. You will need to use ``onIRMessage4(channel)`` event for on a corresponding robot to
        receive the message. Also see the ``getLastIRMessage()`` sensor to keep track of the last message your robot
        received. You can't use a channel for more than one purpose at time, such as sending messages along with
        broadcasting, following, or evading."""
        ToyUtil.send_robot_to_robot_infrared_message(
            self.__toy, bound_value(0, channel, 7), bound_value(1, intensity, 64))

    def listen_for_ir_message(self, channels: Union[int, Iterable[int]], duration: int = 0xFFFFFFFF):
        if isinstance(channels, int):
            channels = (channels,)
        if len(channels) > 0:
            ToyUtil.listen_for_robot_to_robot_infrared_message(
                self.__toy, map(lambda v: bound_value(0, v, 7), channels), bound_value(0, duration, 0xFFFFFFFF))

    def _robot_to_robot_infrared_message_received_notify(self, infrared_code: int):
        self.__last_message = infrared_code
        self.__call_event_listener(EventType.on_ir_message, infrared_code)

    def listen_for_color_sensor(self, colors: Iterable[Color]):
        if self.__toy.implements(IO.set_active_color_palette):
            palette = []
            for i, color in enumerate(colors):
                palette.extend((i, color.r, color.g, color.b))
            if palette:
                self.__toy.set_active_color_palette(palette)

    # Events: are predefined robot functions into which you can embed conditional logic. When an event occurs, the
    # conditional logic is called and then the program returns to the main loop where it left off. The event will
    # be called every time it occurs by default, unless you customize it.
    def __call_event_listener(self, event_type: EventType, *args, **kwargs):
        for f in self.__listeners[event_type]:
            threading.Thread(target=f, args=(self, *args), kwargs=kwargs).start()

    def register_event(self, event_type: EventType, listener: Callable[..., None]):
        """Registers the event type with listener. If listener is ``None`` then it removes all listeners of the
        specified event type.

        **Note**: listeners will be called in a newly spawned thread, meaning the caller have to deal with concurrency
        if needed. This library is thread-safe."""
        if event_type not in EventType:
            raise ValueError('Event type {event_type} does not exist')
        if listener:
            self.__listeners[event_type].add(listener)
        else:
            del self.__listeners[event_type]