import torch
import torch.nn as nn
import torch.nn.functional as F
import numpy as np
import matplotlib.pyplot as plt

class network(nn.Module):

    def __init__(self, rd_layers):
        super().__init__()
        input_size = (192,400)
        assert len(input_size) == 2
        self.kernel_size = 16
        self.size = (192,400)
        self.ord_num = 80
        pretrained = True

        self.retinanet = ResNet102(pretrained=pretrained)
        self.resnet_radar = ResNet_radar(pretrained=pretrained)
  
        self.conv_radar = nn.Sequential(
            nn.Conv2d(2048, 512, kernel_size=1, padding=0, dilation=1),
            nn.ReLU(inplace=True),
            nn.Conv2d(512, 768, kernel_size=1, padding=0, dilation=1),
            nn.ReLU(inplace=True)
        )
        self.concat_process = nn.Sequential(
            nn.Dropout2d(p=0.5),
            nn.Conv2d(1536, 2048, kernel_size=1, padding=0),
            nn.ReLU(inplace=True),
            nn.Dropout2d(p=0.5),
            nn.Conv2d(2048, 80 * 2, kernel_size=1)
        )
        
        self.ord_regression_layer = OrdinalRegressionLayer()
        

    def forward(self, radar, image):

        img_feat = self.retinanet(image)
        radar_feat = self.resnet_radar(radar)

        radar_feat = self.conv_radar(radar_feat)


        feat = torch.cat((img_feat, radar_feat), dim=1)
        #print("Dont become Malik", feat.shape)
        feat = self.concat_process(feat)
        feat = F.interpolate(feat, size=self.size, mode="bilinear", align_corners=True)
        prob = self.ord_regression_layer(feat) # (N, K, H, W)
        
        # calculate label
        label = torch.sum((prob >= 0.5), dim=1).view(-1, 1, 192, 400) # (N, 1, H, W)
    
        return prob, label


affine_par = True


def weights_init(m):
    # Initialize filters with Gaussian random weights
    if isinstance(m, nn.Conv2d):
        n = m.kernel_size[0] * m.kernel_size[1] * m.out_channels
        m.weight.data.normal_(0, math.sqrt(2. / n))
        if m.bias is not None:
            m.bias.data.zero_()
    elif isinstance(m, nn.ConvTranspose2d):
        n = m.kernel_size[0] * m.kernel_size[1] * m.in_channels
        m.weight.data.normal_(0, math.sqrt(2. / n))
        if m.bias is not None:
            m.bias.data.zero_()
    elif isinstance(m, nn.BatchNorm2d):
        m.weight.data.fill_(1)
        m.bias.data.zero_()

def conv3x3(in_planes, out_planes, stride=1):
    "3x3 convolution with padding"
    return nn.Conv2d(in_planes, out_planes, kernel_size=3, stride=stride,
                     padding=1, bias=False)

def conv7x7(in_planes, out_planes, stride=2):
    "3x3 convolution with padding"
    return nn.Conv2d(in_planes, out_planes, kernel_size=7, stride=stride,
                     padding=3, bias=False)


class Bottleneck(nn.Module):
    expansion = 4

    def __init__(self, inplanes, planes, stride=1, dilation=1, downsample=None, fist_dilation=1, multi_grid=1):
        super(Bottleneck, self).__init__()
        self.conv1 = nn.Conv2d(inplanes, planes, kernel_size=1, bias=False)
        self.bn1 = nn.BatchNorm2d(planes)
        self.conv2 = nn.Conv2d(planes, planes, kernel_size=3, stride=stride,
                               padding=dilation * multi_grid, dilation=dilation * multi_grid, bias=False)
        self.bn2 = nn.BatchNorm2d(planes)
        self.conv3 = nn.Conv2d(planes, planes * 4, kernel_size=1, bias=False)
        self.bn3 = nn.BatchNorm2d(planes * 4)
        self.relu = nn.ReLU(inplace=False)
        self.relu_inplace = nn.ReLU(inplace=True)
        self.downsample = downsample
        self.dilation = dilation
        self.stride = stride

    def forward(self, x):
        residual = x

        out = self.conv1(x)
        out = self.bn1(out)
        out = self.relu(out)

        out = self.conv2(out)
        out = self.bn2(out)
        out = self.relu(out)

        out = self.conv3(out)
        out = self.bn3(out)

        if self.downsample is not None:
            residual = self.downsample(x)

        out = out + residual
        out = self.relu_inplace(out)

        return out

class Bottlenecks(nn.Module):
    expansion = 4

    def __init__(self, inplanes, planes, stride=1, downsample=None):
        super(Bottlenecks, self).__init__()
        self.conv1 = nn.Conv2d(inplanes, planes, kernel_size=1, bias=False)
        self.bn1 = nn.BatchNorm2d(planes)
        self.conv2 = nn.Conv2d(planes, planes, kernel_size=3, stride=stride,
                               padding=1, bias=False)
        self.bn2 = nn.BatchNorm2d(planes)
        self.conv3 = nn.Conv2d(planes, planes * 4, kernel_size=1, bias=False)
        self.bn3 = nn.BatchNorm2d(planes * 4)
        self.relu = nn.ReLU(inplace=True)
        self.downsample = downsample
        self.stride = stride

    def forward(self, x):
        residual = x

        out = self.conv1(x)
        out = self.bn1(out)
        out = self.relu(out)

        out = self.conv2(out)
        out = self.bn2(out)
        out = self.relu(out)

        out = self.conv3(out)
        out = self.bn3(out)

        if self.downsample is not None:
            residual = self.downsample(x)

        out += residual
        out = self.relu(out)

        return out

class ResNet(nn.Module):
    def __init__(self, block, layers, in_channel):
        self.inplanes = 128
        super(ResNet, self).__init__()
        self.conv1 = conv3x3(in_channel, 64, stride=2)
        self.bn1 = nn.BatchNorm2d(64, momentum=0.95)
        self.relu1 = nn.ReLU(inplace=False)
        self.conv2 = conv3x3(64, 64)
        self.bn2 = nn.BatchNorm2d(64, momentum=0.95)
        self.relu2 = nn.ReLU(inplace=False)
        self.conv3 = conv3x3(64, 64)
        self.bn3 = nn.BatchNorm2d(64, momentum=0.95)
        self.relu3 = nn.ReLU(inplace=False)
        self.conv4 = conv7x7(64, 128)
        self.bn4 = nn.BatchNorm2d(128, momentum=0.95)
        self.relu4 = nn.ReLU(inplace=False)
        self.maxpool = nn.MaxPool2d(kernel_size=7, stride=4, padding=1)

#         self.relu = nn.ReLU(inplace=False)
#         self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1, ceil_mode=True)  # change
        self.layer1 = self._make_layer(block, 64, layers[0])
        self.layer2 = self._make_layer(block, 128, layers[1])
        self.layer3 = self._make_layer(block, 256, layers[2], dilation=2)
        self.layer4 = self._make_layer(block, 512, layers[3], dilation=4, multi_grid=(1, 1, 1))
        
    def _make_layer(self, block, planes, blocks, stride=1, dilation=1, multi_grid=1):
        downsample = None
        if stride != 1 or self.inplanes != planes * block.expansion:
            downsample = nn.Sequential(
                nn.Conv2d(self.inplanes, planes * block.expansion,
                          kernel_size=1, stride=stride, bias=False),
                nn.BatchNorm2d(planes * block.expansion, affine=affine_par))

        layers = []
        generate_multi_grid = lambda index, grids: grids[index % len(grids)] if isinstance(grids, tuple) else 1
        layers.append(block(self.inplanes, planes, stride, dilation=dilation, downsample=downsample,
                            multi_grid=generate_multi_grid(0, multi_grid)))
        self.inplanes = planes * block.expansion
        for i in range(1, blocks):
            layers.append(
                block(self.inplanes, planes, dilation=dilation, multi_grid=generate_multi_grid(i, multi_grid)))

        return nn.Sequential(*layers)

    def forward(self, x):
        x = self.relu1(self.bn1(self.conv1(x)))
        x = self.relu2(self.bn2(self.conv2(x)))
        x = self.relu3(self.bn3(self.conv3(x)))
        x = self.relu4(self.bn4(self.conv4(x)))
        x = self.maxpool(x)
        x = self.layer1(x)
        x = self.layer2(x)
        x = self.layer3(x)
        x = self.layer4(x)
        return x

    def freeze(self):
        for m in self.modules():
            if isinstance(m, nn.BatchNorm2d):
                m.eval()
 

class ResNet_radar(nn.Module):

    def __init__(self, pretrained=True):
        super().__init__()
        self.backbone = ResNet(Bottleneck, [2,2,2,2], in_channel=7)

    def forward(self, input):
        return self.backbone(input)


class PyramidFeatures(nn.Module):
    def __init__(self, C3_size, C4_size, C5_size, feature_size=256):
        super(PyramidFeatures, self).__init__()

        # upsample C5 to get P5 from the FPN paper
        self.P5_1 = nn.Conv2d(C5_size, feature_size, kernel_size=1, stride=1, padding=0)
        self.P5_upsampled = nn.Upsample(scale_factor=2, mode='nearest')
        self.P5_2 = nn.Conv2d(feature_size, feature_size, kernel_size=3, stride=1, padding=1)

        # add P5 elementwise to C4
        self.P4_1 = nn.Conv2d(C4_size, feature_size, kernel_size=1, stride=1, padding=0)
        self.P4_upsampled = nn.Upsample(scale_factor=2, mode='nearest')
        self.P4_2 = nn.Conv2d(feature_size, feature_size, kernel_size=3, stride=1, padding=1)

        # add P4 elementwise to C3
        self.P3_1 = nn.Conv2d(C3_size, feature_size, kernel_size=1, stride=1, padding=0)
        self.P3_2 = nn.Conv2d(feature_size, feature_size, kernel_size=3, stride=1, padding=1)

        # "P6 is not included for compution (obtained via a 3x3 stride-2 conv on C5)"
        # self.P6 = nn.Conv2d(C5_size, feature_size, kernel_size=3, stride=2, padding=1)

        # # "P7 is not included for compution by applying ReLU followed by a 3x3 stride-2 conv on P6"
        # self.P7_1 = nn.ReLU()
        # self.P7_2 = nn.Conv2d(feature_size, feature_size, kernel_size=3, stride=2, padding=1)

    def forward(self, inputs):
        C3, C4, C5 = inputs

        P5_x = self.P5_1(C5)
        P5_upsampled_x = self.P5_upsampled(P5_x)
        P5_x = self.P5_2(P5_x)

        P4_x = self.P4_1(C4)
        #print("P4_x.shape",P4_x.shape, P5_upsampled_x.shape,P5_x.shape)
        P4_x = P5_x + P4_x
        P4_upsampled_x = self.P4_upsampled(P4_x)
        P4_x = self.P4_2(P4_x)

        P3_x = self.P3_1(C3)
        P3_x = P3_x + P4_x
        P3_x = self.P3_2(P3_x)

        # P6_x = self.P6(C5)

        # P7_x = self.P7_1(P6_x)
        # P7_x = self.P7_2(P7_x)

        return [P3_x, P4_x, P5_x] #P6_x, P7_x]

class RetinaNet(nn.Module):

    def __init__(self, block, layers, in_channel):
        self.inplanes = 128
        super(RetinaNet, self).__init__()
        # self.conv1 = nn.Conv2d(3, 64, kernel_size=7, stride=2, padding=3, bias=False)
        # self.bn1 = nn.BatchNorm2d(64)
        # self.relu = nn.ReLU(inplace=True)

        self.conv1 = conv3x3(in_channel, 64, stride=2)
        self.bn1 = nn.BatchNorm2d(64, momentum=0.95)
        self.relu1 = nn.ReLU(inplace=False)
        self.conv2 = conv3x3(64, 64)
        self.bn2 = nn.BatchNorm2d(64, momentum=0.95)
        self.relu2 = nn.ReLU(inplace=False)
        self.conv3 = conv3x3(64, 128)
        self.bn3 = nn.BatchNorm2d(128, momentum=0.95)
        self.relu3 = nn.ReLU(inplace=False)
        self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1)

        self.convfeat1 = nn.Conv2d(256, 256, kernel_size=7, stride=4, padding=1, bias=False)
        self.convfeat2 = nn.Conv2d(256, 256, kernel_size=7, stride=4, padding=1, bias=False)
        self.convfeat3 = nn.Conv2d(256, 256, kernel_size=7, stride=4, padding=1, bias=False)

        self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1)
        self.layer1 = self._make_layer(block, 64, layers[0])
        self.layer2 = self._make_layer(block, 128, layers[1], stride=1)
        self.layer3 = self._make_layer(block, 256, layers[2], stride=1)
        self.layer4 = self._make_layer(block, 512, layers[3], stride=1)

        if block == Bottlenecks:
            fpn_sizes = [self.layer2[layers[1] - 1].conv3.out_channels, self.layer3[layers[2] - 1].conv3.out_channels,
                         self.layer4[layers[3] - 1].conv3.out_channels]
        else:
            raise ValueError(f"Block type {block} not understood")

        self.fpn = PyramidFeatures(fpn_sizes[0], fpn_sizes[1], fpn_sizes[2])

        self.freeze_bn()

    def _make_layer(self, block, planes, blocks, stride=1):
        downsample = None
        if stride != 1 or self.inplanes != planes * block.expansion:
            downsample = nn.Sequential(
                nn.Conv2d(self.inplanes, planes * block.expansion,
                          kernel_size=1, stride=stride, bias=False),
                nn.BatchNorm2d(planes * block.expansion),
            )

        layers = [block(self.inplanes, planes, stride, downsample)]
        self.inplanes = planes * block.expansion
        for i in range(1, blocks):
            layers.append(block(self.inplanes, planes))

        return nn.Sequential(*layers)

    def freeze_bn(self):
        '''Freeze BatchNorm layers.'''
        for layer in self.modules():
            if isinstance(layer, nn.BatchNorm2d):
                layer.eval()

    def forward(self, inputs):

        x = self.relu1(self.bn1(self.conv1(inputs)))
        x = self.relu2(self.bn2(self.conv2(x)))
        x = self.relu3(self.bn3(self.conv3(x)))
        x = self.maxpool(x)

        x1 = self.layer1(x)
        x2 = self.layer2(x1)
        x3 = self.layer3(x2)
        x4 = self.layer4(x3)

        features = self.fpn([x2, x3, x4])
        feature1 = self.relu1(self.convfeat1(features[0]))
        feature2 = self.relu1(self.convfeat2(features[1]))
        feature3 = self.relu1(self.convfeat3(features[2]))
        #feature4 = self.convfeat4(features[3])
        #feature5 = self.convfeat5(features[4])
        #print(feature1.shape,feature2.shape,feature3.shape)
        cat_Features = torch.cat([feature1,feature2,feature3] , dim=1)
        return cat_Features

        #cat_Features = torch.cat([feature1,feature2,feature3] , dim=1)


class ResNet102(nn.Module):
    def __init__(self, pretrained=True):
        super().__init__()
        self.backbone = RetinaNet(Bottlenecks, [3, 4, 6, 3], in_channel=3)

        if pretrained:
            saved_state_dict = torch.load('/home/mlv/work/DORN_radar/model/resnet50-imagenet.pth',
                                          map_location="cpu")
            new_params = self.backbone.state_dict().copy()
            for i in saved_state_dict:
                i_parts = i.split('.')
#                 print(i_parts)
                if not i_parts[0] == 'fc' and not i_parts[0] == 'conv1' and not i_parts[0] == 'bn1':
                    new_params['.'.join(i_parts[0:])] = saved_state_dict[i]

            self.backbone.load_state_dict(new_params)

    def forward(self, input):
        return self.backbone(input)

class OrdinalRegressionLayer(nn.Module):
    def __init__(self):
        super(OrdinalRegressionLayer, self).__init__()

    def forward(self, x):
        """
        :input x: shape = (N,C,H,W), C = 2*ord_num (2*K)
        :return: ord prob is the label probability of each label, N x OrdNum x H x W
        """
        N, C, H, W = x.size() # (N, 2K, H, W)
        ord_num = C // 2
        
        label_0 = x[:, 0::2, :, :].clone().view(N, 1, ord_num, H, W)  # (N, 1, K, H, W)
        label_1 = x[:, 1::2, :, :].clone().view(N, 1, ord_num, H, W)  # (N, 1, K, H, W)

        label = torch.cat((label_0, label_1), dim=1) # (N, 2, K, H, W)
        label = torch.clamp(label, min=1e-8, max=1e8)  # prevent nans

        label_ord = torch.nn.functional.softmax(label, dim=1)
        prob = label_ord[:,1,:,:,:].clone() # label_ord is the output softmax probability of this model
        return prob
