# import the necessary packages
from __future__ import print_function
import cv2
import time
from time import sleep
from picamera import PiCamera, Color
import numpy as np
from imutils.video.pivideostream import PiVideoStream
from imutils.video import FPS
from picamera.array import PiRGBArray
from skimage.transform import resize
import matplotlib.pyplot as plt
import argparse
import imutils
import os, os.path
from glob import glob

import RPi.GPIO as GPIO

DEBUG = 1
 
# read SPI data from MCP3008 chip, 8 possible adc's (0 thru 7)
def readadc(adcnum, clockpin, mosipin, misopin, cspin):
        if ((adcnum > 7) or (adcnum < 0)):
                return -1
        GPIO.output(cspin, True)
 
        GPIO.output(clockpin, False)  # start clock low
        GPIO.output(cspin, False)     # bring CS low
 
        commandout = adcnum
        commandout |= 0x18  # start bit + single-ended bit
        commandout <<= 3    # we only need to send 5 bits here
        for i in range(5):
                if (commandout & 0x80):
                        GPIO.output(mosipin, True)
                else:
                        GPIO.output(mosipin, False)
                commandout <<= 1
                GPIO.output(clockpin, True)
                GPIO.output(clockpin, False)
 
        adcout = 0
        # read in one empty bit, one null bit and 10 ADC bits
        for i in range(12):
                GPIO.output(clockpin, True)
                GPIO.output(clockpin, False)
                adcout <<= 1
                if (GPIO.input(misopin)):
                        adcout |= 0x1
 
        GPIO.output(cspin, True)
        
        adcout >>= 1       # first bit is 'null' so drop it
        return adcout



# SET UP PINS AND PIN STATES
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(16,GPIO.IN) # shutter switch pin
GPIO.setup(20,GPIO.IN) # playback switch pin

SPICLK = 18  # ADC clock
SPIMISO = 23 # ADC data out
SPIMOSI = 24 # ADC control in
SPICS = 25   # ACD CS

GPIO.setup(SPIMOSI, GPIO.OUT) 
GPIO.setup(SPIMISO, GPIO.IN)
GPIO.setup(SPICLK, GPIO.OUT)
GPIO.setup(SPICS, GPIO.OUT)

buttonState = 1 # initalize state of shutter switch
prevState = 0   # initalize previous state of shutter switch

buttonState2 = 0 # initalize state of playback switch
prevState2 = 0   # initalize previous state of playback switch

foldOut = '/home/pi/Desktop/spaceTimeCode/Images/' # folder images are saved to

# TRIM POT CONDITIONS
last_read = 0       # this keeps track of the last potentiometer value
tolerancePlayBack = 7   # tolarance of trimpot before changing image in playback
toleranceLine = 5       # tolarance of trimpot before changing line
maxPot = 1000           # Max trim pot value
potentiometer_adc = 0


# SET UP VIDEO DISPLAY
# Screen dimensions: 88 x 154 mm (ratio of 7/4 = 1.75)
ratScreen = float(800)/(480) # ratio of LCD screen dimension
nPy = int(480) # number of vertical pixels for display image
nPx = int(ratScreen*nPy) # number of horiztonal pixels for display image

nPyE=int(240) # number of vertical pixels for image captured 
nPxE=int(ratScreen*nPyE) # number of horizontal pixels for image captured 256; # nPyE=160;

indDiff=(nPxE-nPx/2)/2; # pixel difference between image captured and image displayed
indArr = np.arange(indDiff,(indDiff+nPx/2)); 

A = np.zeros((nPy,nPx,3), dtype=np.uint8) # Frame to display
At = np.zeros((nPyE,nPxE,3), dtype=np.uint8) # Captured frame
ST = np.zeros((nPy,nPx/2,3), dtype=np.uint8) # Space time frame

vs = PiVideoStream(resolution=(nPxE,nPyE),framerate=30).start() # set up pi stream object
time.sleep(2.0) 
fps = FPS().start()

# SET UP SPACE LINE
last_read = readadc(potentiometer_adc, SPICLK, SPIMOSI, SPIMISO, SPICS)
z=0;

LineCapture = int(float(last_read)/maxPot*nPxE);
LineCaptureLoop =False
if (LineCapture>nPxE-1):
    LineCaptureLoop = True


# READ IN SAVED IMAGE FILENAMES
files_list = glob(os.path.join(foldOut, '*.jpg'))
numPicDir = len(files_list)
fileNum = []

for a_file in sorted(files_list):
    d=filter(str.isdigit, a_file)
    fileNum.append(d)

fileNum=sorted(fileNum, key=int)

if not fileNum:
    p=1
    fileNum = '1';
else:
    p = int(fileNum[numPicDir-1])+1 # number of first image to be saved

#MAIN CODE
while True:

    # LIVE SPACE TIME MODE 
    At = vs.read(); # capture image from pi camera
    At = imutils.resize(At, width=nPx) # resize image
    At = At[:,indDiff:(indDiff+nPx/2),:] # crop image to be a square


    if (LineCaptureLoop == True): # if true, column increases automatically with each frame
        LineCapture = z
    
    ST[:,z,:] = At[:,LineCapture,:] # Take column of captured image and store it in space-time image

    A[0:nPy,0:nPx/2,:]=At # Set left half of frame to image captured
    A[0:nPy,nPx/2:nPx,:]=ST # Set the other half frame to space-time image
    A[:,LineCapture:LineCapture+2,:]=[0, 0, 255] # Color space-line red
            
    cv2.imshow("test",A) # Display image

    buttonState = GPIO.input(16) # read shutter switch
    buttonState2 = GPIO.input(20) # read playback switch
    trim_pot = readadc(potentiometer_adc, SPICLK, SPIMOSI, SPIMISO, SPICS) # read analog input of trim
    pot_adjust = abs(trim_pot - last_read) # difference in trim pot
    last_read = trim_pot # Save reading on trim
    
    if (pot_adjust > toleranceLine): # trim pot passes tolerance, update space-line position
        
        LineCapture = int(float(trim_pot)/maxPot*nPxE) # normalize trim pot reading and convert to column position 
        
        if (LineCapture>nPxE-1): # Space-line is greater than number of columns, set to continuous line shift
            LineCaptureLoop = True
        elif (LineCapture<=nPxE-1):     
            LineCaptureLoop = False

    if (buttonState ==0 and prevState==1): # Shutter has been pressed and was not pressed on last cycle, therefore take picture
        # Save photo
        prevState = 0
        filename = foldOut+'image'+str(p)+'.jpg' # File location of saved image
        cv2.imwrite(filename,A) # Save photo
        p+=1 # increment p for saving photos

    elif (buttonState==1 and prevState==0): # button has been released, set previous position to high
        prevState = 1
        
    # LIVE SPACE TIME MODE END

    
    # PLAYBACK MODE
    if (buttonState2 ==1 and prevState2==0): # playback button has been pressed
        # enter playback mode
        files_list = glob(os.path.join(foldOut, '*.jpg')) # get list of all photos in save photo directory
        numPicDir = len(files_list) # get number of pictures saved to disc
        fileNum = []

        for a_file in sorted(files_list):
            d=filter(str.isdigit, a_file)
            fileNum.append(d)
        fileNum=sorted(fileNum, key=int) # filename numbers of photos saved to disc

        if not fileNum: # fileNum is empty
            # no photos saved!
            p=1
            fileNum = '1';
            playBackCond = False # Set playback to false and return to live space-time stream
        else:
            p = int(fileNum[numPicDir-1])+1 # number of first image to be saved
            playBackCond = True
            prevState2 = 1
            whichFrame = 0
            filename = foldOut+'image'+fileNum[whichFrame]+'.jpg' # name of first image file to display in playback
            img = cv2.imread(filename) # load playback file
            cv2.imshow("test",img) # display playback file
            last_read = readadc(potentiometer_adc, SPICLK, SPIMOSI, SPIMISO, SPICS) # read trim pot

        
        while (playBackCond == True):

            buttonState2 = GPIO.input(20) # read play back button to check if user wants to leave playback mode
    
            trim_pot = readadc(potentiometer_adc, SPICLK, SPIMOSI, SPIMISO, SPICS) # read the analog pin
            pot_adjust = abs(trim_pot - last_read)
            
            if (buttonState2==0 and prevState2==1): # once button is released set prevState to LOW
                prevState2 = 0
            
            if (buttonState2==1 and prevState2==0): # button pressed again, leave playback mode
                playBackCond = False # user clicked playback button again and she wants to return to live space-time stream
            
            if (pot_adjust > tolerancePlayBack):# user turned knob
                
                if(trim_pot>last_read): # user turned knob to right
                    whichFrame = whichFrame +1 # cycle through saved images to right

                elif(trim_pot<last_read): # user turned knob to left
                    whichFrame = whichFrame -1 # cycle through saved images to left

                if(whichFrame>numPicDir-1) or (trim_pot>maxPot): # user has reached last image
                    whichFrame = numPicDir-1

                elif(whichFrame<0) or (trim_pot<0): # user has reached first image
                    whichFrame = 0
 
            last_read = trim_pot # save trim pot value
            
            
            filename = foldOut+'image'+fileNum[whichFrame]+'.jpg' # filename of saved image to display
            img = cv2.imread(filename) # load image
            cv2.imshow("test",img) # display image
            cv2.waitKey(50) # pause

            buttonState = GPIO.input(16) # read shutter button which is now delete button
                
            if (buttonState ==0 and prevState==1): # delete image if shutter is clicked...
                # delete photo
                prevState = 0
                filename = foldOut+'image'+fileNum[whichFrame]+'.jpg' # filename of image to be deleted
                os.remove(filename) # delete image
                fileNum[whichFrame:whichFrame+1] = [] # remove image from file list
                whichFrame = whichFrame -1 
                numPicDir = numPicDir - 1
                if (numPicDir == 0): # no pictures left saved on disc
                    playBackCond = False # return to live ST MODE
                time.sleep(0.1)
                

                if(whichFrame<0): # user has reached first image
                    whichFrame = 0

            elif (buttonState==1 and prevState==0):
                prevState = 1

        prevState2 = 1
    # PLAYBACK MODE END


    if (buttonState2==0 and prevState2==1): # once button is released set prevState to LOW
        prevState2 = 0
    
    z = z+1
    if z >nPx/2-1:
        z=0

    if cv2.waitKey(1) & 0xFF==ord('q'):
        cv2.namedWindow("image",cv2.WINDOW_NORMAL)
        cv2.destroyAllWindows()
        GPIO.cleanup()
        break

    time.sleep(0.01)

cv2.destroyAllWindows()
vs.stop()