import argparse
import cv2
import numpy

helpText = "'Esc' to Quit"

def parse_args():
    parser = argparse.ArgumentParser(description="CV2 Video Capture PhotoPlethysmoGram Test")
    parser.add_argument("--vid", dest="video_dev",
                        help="video device # of USB webcam (/dev/video?) [1]",
                        default=0, type=int)
    args = parser.parse_args()
    return args

def line(x0, x1, y0, y1, func):
    isSwap = abs(y1 - y0) > abs(x1 - x0)
    if isSwap:
        x0, y0 = y0, x0
        x1, y1 = y1, x1
    if x0 > x1:
        x0, x1 = x1, x0
        y0, y1 = y1, y0
    deltax = x1 - x0
    deltay = abs(y1 - y0)
    error = deltax / 2
    y = y0
    if y0 < y1:
        ystep = 1
    else:
        ystep = -1
    for x in range(x0, x1):
        if isSwap:
            func(y, x)
        else:
            func(x, y)
        error = error - deltay
        if error < 0:
            y = y + ystep
            error = error + deltax

def plot_r(x, y):
    frame[int(y), int(x)] = [255,0,0]
def plot_g(x, y):
    frame[int(y), int(x)] = [0,255,0]
def plot_b(x, y):
    frame[int(y), int(x)] = [0,0,255]
def plot_w(x, y):
    frame[int(y), int(x)] = [255,255,255]

if __name__ == "__main__":
    args = parse_args()
    print("Called with args:")
    print(args)
    cap = cv2.VideoCapture(args.video_dev)
    width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
    px = int(width /2 )  # float
    py = int(height /2 ) # float
    print(py,px)

    drawDataList = []

    while(True):
        ret, frame = cap.read()
        if ret:
          cv2.putText(frame, helpText, (11,20), cv2.FONT_HERSHEY_PLAIN, 1.0, (32,32,32), 4, cv2.LINE_AA)
          cv2.putText(frame, helpText, (10,20), cv2.FONT_HERSHEY_PLAIN, 1.0, (240,240,240), 1, cv2.LINE_AA)
          #getPixel = frame[py,px]
          #print (getPixel)
          avg_color_per_row = numpy.average(frame, axis=0)
          avg_color = numpy.average(avg_color_per_row, axis=0)
          #drawDataList.append(frame[py,px])
          drawDataList.append(avg_color)
          if len(drawDataList) > width/4 :
              drawDataList.pop(0)

          now_x = 0
          old_x = 0
          old_ry = 0
          old_gy = 0
          old_by = 0
          for p in drawDataList:
              by = int((255-p[0]) * 1.5)
              gy = int((255-p[1]) * 1.5)
              ry = int((255-p[2]) * 1.5)
              if now_x == 0:
                  frame[old_by, now_x] = [255,0,0]
                  frame[old_gy, now_x] = [0,255,0]
                  frame[old_ry, now_x] = [0,0,255]
              else:
                  line(old_x, now_x, old_by, by, plot_r)
                  line(old_x, now_x, old_gy, gy, plot_g)
                  line(old_x, now_x, old_ry, ry, plot_b)
                  line(old_x+1, now_x+1, old_ry+1, ry+1, plot_b)
                  line(old_x+2, now_x+2, old_ry+2, ry+2, plot_w)
                  #dx = now_x - old_x
                  ##dx = 1
                  #dby = by - old_by
                  #dgy = gy - old_gy
                  #dry = ry - old_ry

                  #for y in range(old_by, by, 1 if old_by < by else -1):
                  #    x = old_x + (dx * (y - old_by)) / dby
                  #    frame[int(y), int(x)] = [255,0,0]
                  #for y in range(old_gy, gy, 1 if old_gy < gy else -1):
                  #    x = old_x + (dx * (y - old_gy)) / dgy
                  #    frame[int(y), int(x)] = [0,255,0]
                  #for y in range(old_ry, ry, 1 if old_ry < ry else -1):
                  #    x = old_x + (dx * (y - old_ry)) / dry
                  #    frame[int(y), int(x)] = [0,0,255]

              old_x = now_x
              old_by = by
              old_gy = gy
              old_ry = ry
              now_x=now_x+2

          cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == 27:
            break

    cap.release()
    cv2.destroyAllWindows()