import argparse
import time
from pathlib import Path

from PIL import Image
import cv2
import torch
import torch.backends.cudnn as cudnn
from numpy import random
import numpy as np

from LD import LaneDetection
from models.experimental import attempt_load
from utils.datasets import LoadStreams, LoadImages
from utils.general import check_img_size, check_requirements, check_imshow, non_max_suppression, apply_classifier, \
    scale_coords, xyxy2xywh, strip_optimizer, set_logging, increment_path
from utils.plots import plot_one_box
from utils.torch_utils import select_device, load_classifier, time_synchronized

def detect(save_img=False):
    source, weights, view_img, save_txt, imgsz = opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size
    save_img = not opt.nosave and not source.endswith('.txt')  # save inference images
    webcam = source.isnumeric() or source.endswith('.txt') or source.lower().startswith(
        ('rtsp://', 'rtmp://', 'http://', 'https://'))

    # Directories
    save_dir = Path(increment_path(Path(opt.project) / opt.name, exist_ok=opt.exist_ok))  # increment run
    (save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True)  # make dir

    # Initialize
    set_logging()
    device = select_device(opt.device)
    half = device.type != 'cpu'  # half precision only supported on CUDA

    # Load model
    model = attempt_load(weights, map_location=device)  # load FP32 model
    stride = int(model.stride.max())  # model stride
    imgsz = check_img_size(imgsz, s=stride)  # check img_size
    if half:
        model.half()  # to FP16

    # Second-stage classifier
    classify = False
    if classify:
        modelc = load_classifier(name='resnet101', n=2)  # initialize
        modelc.load_state_dict(torch.load('weights/resnet101.pt', map_location=device)['model']).to(device).eval()

    # Set Dataloader
    vid_path, vid_writer = None, None
    if webcam:
        view_img = check_imshow()
        cudnn.benchmark = True  # set True to speed up constant image size inference
        dataset = LoadStreams(source, img_size=imgsz, stride=stride)
    else:
        dataset = LoadImages(source, img_size=imgsz, stride=stride)

    # Get names and colors
    names = model.module.names if hasattr(model, 'module') else model.names
    colors = [[random.randint(0, 255) for _ in range(3)] for _ in names]

    # # input lane detection model
    # LaneDetection_yolo = LaneDetection()

    # Run inference
    if device.type != 'cpu':
        model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters())))  # run once
    t0 = time.time()
    for path, img, im0s, vid_cap in dataset:

        img = torch.from_numpy(img).to(device)
        img = img.half() if half else img.float()  # uint8 to fp16/32
        img /= 255.0  # 0 - 255 to 0.0 - 1.0
        if img.ndimension() == 3:
            img = img.unsqueeze(0)

        # Inference
        t1 = time_synchronized()
        pred = model(img, augment=opt.augment)[0]

        # Apply NMS
        pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, classes=opt.classes, agnostic=opt.agnostic_nms)
        t2 = time_synchronized()

        # Apply Classifier
        if classify:
            pred = apply_classifier(pred, modelc, img, im0s)

        # Process detections
        for i, det in enumerate(pred):  # detections per image
            if webcam:  # batch_size >= 1
                p, s, im0, frame = path[i], '%g: ' % i, im0s[i].copy(), dataset.count
            else:
                p, s, im0, frame = path, '', im0s, getattr(dataset, 'frame', 0)

            # Utilize Lane Detection
            left_lines_fitted, right_lines_fitted = LaneDetection.LD(picture = im0)


            p = Path(p)  # to Path
            save_path = str(save_dir / p.name)  # img.jpg
            txt_path = str(save_dir / 'labels' / p.stem) + ('' if dataset.mode == 'image' else f'_{frame}')  # img.txt
            s += '%gx%g ' % img.shape[2:]  # print string
            gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
            if len(det):
                # Rescale boxes from img_size to im0 size
                det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()

                # Print results
                for c in det[:, -1].unique():
                    n = (det[:, -1] == c).sum()  # detections per class
                    s += f"{n} {names[int(c)]}{'s' * (n > 1)}, "  # add to string

                # Write results
                for *xyxy, conf, cls in reversed(det):
                    if save_txt:  # Write to file
                        xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  # normalized xywh
                        line = (cls, *xywh, conf) if opt.save_conf else (cls, *xywh)  # label format
                        with open(txt_path + '.txt', 'a') as f:
                            f.write(('%g ' * len(line)).rstrip() % line + '\n')

                    if save_img or view_img:  # Add bbox (and lane) to image
                        label = f'{names[int(cls)]} {conf:.2f}'
                        plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)

            # Print time (inference + NMS)
            print(f'{s}Done. ({t2 - t1:.3f}s)')

            # Print in the lanes and stream results
            if view_img:
                if left_lines_fitted.shape == (2,2):
                    if (left_lines_fitted[1][1]-left_lines_fitted[0][1]) < (-0.35)*(left_lines_fitted[1][0]-left_lines_fitted[0][0])-0.01:
                        cv2.line(im0, tuple(left_lines_fitted[0]), tuple(left_lines_fitted[1]), color = (255,0,255),
                                 thickness=5)                                                           # purple
#-----------------------Left line Marker---------------------------------------------------
                        cv2.line(im0, (left_lines_fitted[1][0]+left_lines_fitted[0][0]-610, 400), (left_lines_fitted[1][0]+left_lines_fitted[0][0]-610,680), color = (255,255,205),
                                 thickness=2)
#------------------------------------------------------------------------------------------

#------------------------Mid line marker-------------------------------------------------------   
                        cv2.line(im0,(left_lines_fitted[1][0]+left_lines_fitted[0][0]-610, 680) , (right_lines_fitted[1][0]-right_lines_fitted[0][0]+660, 680), color = (255,255,105),
                                 thickness=2)
                        cv2.line(im0,(left_lines_fitted[1][0]+left_lines_fitted[0][0]-610, 400) , (right_lines_fitted[1][0]-right_lines_fitted[0][0]+660, 400), color = (255,255,105),
                                 thickness=2)
#------------------------------------------------------------------------------------------   

                if right_lines_fitted.shape == (2,2):
                    if (right_lines_fitted[1][1]-right_lines_fitted[0][1]) > 0.35*(right_lines_fitted[1][0]-right_lines_fitted[0][0])+0.01:
                        cv2.line(im0, tuple(right_lines_fitted[0]), tuple(right_lines_fitted[1]), color = (0,255,255),
                                 thickness=5)                                                          # yellow
#-----------------------Right line Marker--------------------------------------------------
                        cv2.line(im0,(right_lines_fitted[1][0]-right_lines_fitted[0][0]+660,400) , (right_lines_fitted[1][0]-right_lines_fitted[0][0]+660, 680), color = (255,255,205),
                                 thickness=2)
                        
#------------------------Mid line marker-------------------------------------------------------   
                        cv2.line(im0,(left_lines_fitted[1][0]+left_lines_fitted[0][0]-610, 680) , (right_lines_fitted[1][0]-right_lines_fitted[0][0]+660, 680), color = (255,255,105),
                                 thickness=2)
                        cv2.line(im0,(left_lines_fitted[1][0]+left_lines_fitted[0][0]-610, 400) , (right_lines_fitted[1][0]-right_lines_fitted[0][0]+660, 400), color = (255,255,105),
                                 thickness=2)
#----------------------- target marker------------------------------------------------------------------   
                        cv2.line(im0,tuple(left_lines_fitted[1]) , tuple(right_lines_fitted[0]), color = (255,255,105),
                                 thickness=2)


                cv2.imshow(str(p), im0)
                cv2.waitKey(1)  # 1 millisecond

#------------------------------------------------------------------------------------------  
                
                print(right_lines_fitted[1][0]-right_lines_fitted[0][0]+640)
                print(left_lines_fitted[0][1]+left_lines_fitted[0][0]-680)
            
#------------------------------------------------------------------------------------------                  

            # Save results (image with detections)
            if save_img:
                if dataset.mode == 'image':
                    cv2.imwrite(save_path, im0)
                else:  # 'video' or 'stream'
                    if vid_path != save_path:  # new video
                        vid_path = save_path
                        if isinstance(vid_writer, cv2.VideoWriter):
                            vid_writer.release()  # release previous video writer
                        if vid_cap:  # video
                            fps = vid_cap.get(cv2.CAP_PROP_FPS)
                            w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
                            h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
                        else:  # stream
                            fps, w, h = 30, im0.shape[1], im0.shape[0]
                            save_path += '.mp4'
                        vid_writer = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h))
                    vid_writer.write(im0)

    if save_txt or save_img:
        s = f"\n{len(list(save_dir.glob('labels/*.txt')))} labels saved to {save_dir / 'labels'}" if save_txt else ''
        print(f"Results saved to {save_dir}{s}")

    print(f'Done. ({time.time() - t0:.3f}s)')


if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument('--weights', nargs='+', type=str, default='yolov5s.pt', help='model.pt path(s)')
    # parser.add_argument('--source', type=str, default='0', help='source')
    parser.add_argument('--source', type=str, default='runs/video/Drive_01.mp4', help='source')
    #                         # (0 for webcam)!!!!!!!
    parser.add_argument('--img-size', type=int, default=640, help='inference size (pixels)')
    parser.add_argument('--conf-thres', type=float, default=0.7, help='object confidence threshold')
                            # confidence
    parser.add_argument('--iou-thres', type=float, default=0.45, help='IOU threshold for NMS')
                            # IoU (intersection over union)
                            # NMS
    parser.add_argument('--device', default='0', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
    parser.add_argument('--view-img', default=True, help='display results')
                            # real time view
    parser.add_argument('--save-txt', default=True, help='save results to *.txt')
    parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')
    parser.add_argument('--nosave', action='store_true', help='do not save images/videos')
    parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --class 0, or --class 0 2 3')
                            # show classes
    parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
                            # augment nms
    parser.add_argument('--augment', default=True, help='augmented inference')
                            # augment detection
    parser.add_argument('--update', action='store_true', help='update all models')
    parser.add_argument('--project', default='runs/detect', help='save results to project/name')
                            # place to save
    parser.add_argument('--name', default='exp_detect', help='save results to project/name')
                            # name of the file to save
    parser.add_argument('--exist-ok', action='store_true', help='existing project/name ok, do not increment')
                            # whether create a new file or not
    opt = parser.parse_args()
    print(opt)
    check_requirements(exclude=('pycocotools', 'thop'))

    with torch.no_grad():
        if opt.update:  # update all models (to fix SourceChangeWarning)
            for opt.weights in ['yolov5s.pt', 'yolov5m.pt', 'yolov5l.pt', 'yolov5x.pt']:
                detect()
                strip_optimizer(opt.weights)
        else:
            detect()
