PPG_processing_functions

# PPG function list:

# 1. Preproc  (note for bioradio data only)
# 2. find_w   
# 3. find_u_v
# 4. find_o
# 5. preclean_wuv
# 6. Baseline
# 7. Clean_wuv
# 8. sep_beats
# 9. find_average
# 10. find_sd
# 11. diast_pk     
# 12. osnd_of_average
# 13. feature_extract


preproc <- function(data){
  dat<-data[!(data$PPG.PulseOx1=='NaN'),]
  
  #Downsample
  # The BioRadio device provides 250 samples per second, but the PPG is only sampled 75 times per second, 
  # so we typically have repeated values in a pattern of 3-3-4 repeating. DownSample tries to retrieve the 
  # unique values, with an effort to be robust against variation in the repeat pattern and also against 
  # genuine repeated values.
  
  list<-rle(dat$PPG.PulseOx1)
  ID <- rep(1:length(list$values), times = list$lengths)
  data_downsampled <- c()
  
  nSrc <- nrow(dat)
  iDst <- 1
  iSrc <- 1
  iVal <- 1
  print("Deduplicating data...")
  if(TRUE){ # Mode switch in case new method is worse or somehow broken
    startClock <- Sys.time()
    reportClock <- 10
    
    data_downsampled <- cbind(dat, ID)
    
    while (iSrc <= nSrc){
      if (as.double(difftime(Sys.time(),startClock,unit="secs"))>reportClock){
        min <- as.integer(reportClock/60)
        sec <- reportClock %% 60
        time <- paste("[",min,":",sep="")
        if (sec == 0){ time <- paste(time,"00]",sep="")} else { time <- paste(time,sec,"]",sep="")}
        print(paste(time,iSrc,"of",nSrc))
        reportClock <- reportClock + 10
      }
      
      data_downsampled[iDst,] <- data_downsampled[iSrc,]
      iDst <- iDst + 1
      if (list$lengths[iVal] <= 4){
        iSrc <- iSrc + list$lengths[iVal]
        iVal <- iVal + 1
      } else {
        iSrc <- iSrc + 4
        list$lengths[iVal] <- list$lengths[iVal] - 4
      }
    }
    
    # Trim downsampled data to size
    data_downsampled <- data_downsampled[1:(iDst-1),]
    
  }else{ # Old method, using rbind which gets slow
    
    data2 <- cbind(dat, ID)
    data_downsampled <-c()
    
    for (i in 1:max(ID)){
      sub.data <- dplyr::filter(data2, ID == i)
      if(nrow(sub.data) <= 4){
        data_downsampled <- rbind(data_downsampled, sub.data[1,])
      }else if(nrow(sub.data) > 4 ){data_downsampled <- rbind(data_downsampled, sub.data[1,], sub.data[5,])}
    }
    
  } # End of method selector
  
  print("Removing DC blocker...")
  #Undetrend
  # Analysis of device output indicates that the PPG signal is detrended by application of the following
  # formula: OUT[i] = 80 + (OUT[i-1]-80) * 0.96875 + (IN[i] - [IN[i-1]), where the constant 0.96875 is
  # an approximation fitted to the data.
  # Individual pulse events are more comprehensible if the detrending is not used, so this function 
  #removes it by inverting the above function. 
  undetrended <-replicate(length(data_downsampled$PPG.PulseOx1)-1, 0) 
  undetrended<-c(data_downsampled$PPG.PulseOx1[1],undetrended) #add first detrended value to vector
  for (i in 2:length(data_downsampled$PPG.PulseOx1)){
    undetrended[i]<-((data_downsampled$PPG.PulseOx1[i]-80) - ((data_downsampled$PPG.PulseOx1[i-1]-80) * 0.96875) + (undetrended[i-1]))
  }
  print("Done")
  return(undetrended)
}



find_w <- function(d1p, deriv1, sp, sr){
  ########################################################################################################################################
  # FindW identifies peaks in the first derivative of the PPG time series (denoted w on the original PPG pulse wave (Elgendi et al, 2018)).  
  # A rolling window relative to heart rate is applied to identify beats and artefacts. 
  # Peaks identified within a window are confirmed as genuine / artefactual with a series of checks against thresholds derived from beats 
  # local to the window and across the entire time series. 
  
  # Inputs: 
  # d1p (polynomial spline of first derivative of the PPG time series)
  # deriv1 (first derivative in discrete form)
  # sp (polynomial spline of original time series)
  # sr (sample rate)
  
  # Outputs:
  # wX (x axis coordinates of all w points) 
  # wY (y axis coordinates of all w points on original PPG trace)
  # wYD1 (y axis coordinates of all w points on 1st derivative trace)
  ########################################################################################################################################
  
  d1InflxX <- solve(d1p, b = 0, deriv = 1)                 # Find inflection points on 1st deriv
  d1InflxY <- predict(d1p, d1InflxX)
  wX <- c()                                                # Create vectors for w values to be stored
  wY <- c()
  window <- list()                                         # Define window within which to identify peaks 
  prevPkDist <- c()                                        # Create vector to store peak to peak distances
  
  ############# Identify first two peaks: #############
  
  a <- 2                                                                                                                                        # Start with a very small window (spanning three (1 + a) inflection points), 
  while(length(wX) < 2){                                                                                                                        # and widen until two peaks are identified within the window
    
    firstWindow <- data.frame(d1InflxX[1]:d1InflxX[1+a], deriv1[d1InflxX[1]:d1InflxX[1+a]])                                                     # Create window
    windowInflxY <- d1InflxY[which(d1InflxX > firstWindow[1, 1] & d1InflxX < firstWindow[, 1][length(firstWindow[, 1])])]                       # Find the inflection points which fall within the window 
    windowInflxX <- d1InflxX[which(d1InflxX > firstWindow[1, 1]  & d1InflxX < firstWindow[, 1][length(firstWindow[, 1])])]                     
    threshold <- quantile(firstWindow[, 2], probs=c(.95))                                                                                       # Define a threshold for finding peaks, of 0.95 
    windowPks <- which(windowInflxY > threshold)                                                                                                # Identify inflection points above the threshold as peaks
                                                                                                                                                       
    if(length(windowPks) == 2 & windowInflxX[windowPks[2]] - windowInflxX[windowPks[1]] > sr){                                                                                      # If the first two peaks indicate a heart rate < 60 bpm, 
      confirmWindow <- data.frame(d1InflxX[windowPks[1]+2]:d1InflxX[windowPks[2]-2],   deriv1[d1InflxX[windowPks[1]+2]:d1InflxX[windowPks[2]-2]])       # ensure a peak between them has not been missed with the current threshold
      inflxConformY <- d1InflxY[which(d1InflxX > confirmWindow[1, 1] & d1InflxX < confirmWindow[, 1][length(confirmWindow[, 1])])]
      inflxConformX <- d1InflxX[which(d1InflxX > confirmWindow[1, 1]  & d1InflxX < confirmWindow[, 1][length(confirmWindow[, 1])])]                     # Find inflection points within the window (defined as between peaks 1 and 2)
      threshold <- max(quantile(confirmWindow[, 2], probs=c(.95)), windowInflxY[windowPks[1]]/2)     
      missedPks <- inflxConformX[which(inflxConformY > threshold)]                                                                                      # Identify peaks above threshold (and greater than 50% of the height of the first peak)
      missedPks <- which(windowInflxX == missedPks[1])                                                                                                  # Identify the inflection point in the original window which corresponds to the missed peak 
      if(length(missedPks) > 0){windowPks[2] <- missedPks[1]}                                                                                                                       # Assign the missed peak as the second peak
    }
    
    if(length(windowPks) == 2 & windowInflxX[windowPks[2]] - windowInflxX[windowPks[1]] < (sr/10)){                                                                         # Rarely, the first peak contains two inflection points above threshold, 
      windowPks <- windowPks[-2]                                                                                                                # if peaks are too close (< 1/10th of a second), the second peak is discounted
    }
    
    windowPksY <- windowInflxY[windowPks]                                                                                                       # Identify the inflection points corresponding to peaks
    windowPks <- windowInflxX[windowPks]     
    
    if(length(windowPks) > 2 & (windowPks[3] - windowPks[1] < 1)){                                                                              # Very rarely, the first peak contains three inflection points above threshold. Discount two. 
      windowPks <- windowPks[-c(1, 3)]
    }
    
    m <- mean(d1InflxY[1:(1+a)])                                                                                                                # Calculate mean and SD of inflection points within the window
    std <- sd(d1InflxY[1:(1+a)])
    
    if(length(windowPks) == 2){                                                                                                                 # If two peaks are found, confirm that they are both significantly higher than surrounding                                                                                 
      if(windowPksY[1] > (m + (1.5*std)) & windowPksY[1] > (median(deriv1[1:100])+std(deriv1[1:100]))){                                         # inflection points, and that the second peak is greater than half the height of the first     
        wX[1] <-  windowPks[1]                                                                                                                  # peak (unless the first peak is an artefact)
        if(windowPksY[2] > (m + (1.5*std)) & windowPksY[2] > (windowPksY[1]/2) | windowPksY[1] > (mean(deriv1) + (5*std(deriv1)))){  
          wX[2] <-  windowPks[2]                                                                                                                          
        }
      }
    }   
    
    if(length(windowPks) > 3){                                                                                                                  # If more than 3 peaks identified, assume the time series begins with an artefact and skip 
      d1InflxX <- d1InflxX[-c(1:(sr*(100/75)))]                                                                                                 # forward, resetting a. 
      d1InflxY <- d1InflxY[-c(1:(sr*(100/75)))]
      a <- -3  
      print("skipped")
    }
    
    a <- a + 5                                                                                                                                  # Increases until two legitimate peaks are found
  }
  
  ############# Find remaining peaks (with dynamic moving window dependent on peak to peak distance): #############
  
  artefacts <- c()                                                                                                                              # Create vector to store artefactual peaks
  m <- mean(d1InflxY)                                                                                                                           # Calculate mean and SD of inflection points across the time series
  std <- sd(d1InflxY)
  for(i in 3:length(d1InflxX)){  
    prevPkDist[i] <- wX[length(wX)] - wX[length(wX)-1]                                                                                          # Peak to peak distance defined as distance between two previous peaks
    if(prevPkDist[i] > (mean(prevPkDist[!is.na(prevPkDist)])*2)){                                                                               # Identify abnormally large peak to peak distances (this can be because 
      prevPkDist[i] <- prevPkDist[3]                                                                                                            # there was a large gap between an artefact and the next legitimate peak) 
    }                                                                                                                                           # and correct them by resetting the value (this avoids leapfrogging)     
    if((wX[length(wX)] + (4*prevPkDist[i]) > length(deriv1))){                            
      break                                                                                                                                     # If the next window goes beyond the length of the data, break the loop
    }                                      
    windowPks <- c()
    windowExtnd <- 1.35                                                                                                                         # Define parameters by which to extend the window or move it forward
    windowStart <- 0.5
    printed <- NA
    if(length(artefacts) > 0){                                                                                                                  # Recalculate mean and standard deviation after removing any artefacts identified 
      remove <- c()
      for(j in artefacts[which(artefacts < length(wX))]){
        remove[j] <- which(abs(d1InflxX - wX[j]) == min(abs(d1InflxX - wX[j])))
      }
      remove <- remove[!is.na(remove)]
      newRem <- c()
      for(j in 1:length(remove)){
        newRem[(length(newRem)+1):(length(newRem)+21)] <- (remove[j] -10): (remove[j] + 10)                                                     # Remove inflection points around artefact peaks
      }
      if(sum(newRem < 1) > 0){  
        newRem <- newRem[-(which(newRem < 1))] 
      }    
      m <- mean(d1InflxY[-newRem])       
      std <- sd(d1InflxY[-newRem])  
    }
    
    while(length(windowPks) < 1){                                                                                                               # Each subsequent window will adjust (if required) until the next peak is detected
      
      if(windowExtnd > 10){                  
        windowStart <- 2
        windowExtnd <- 2.5
      }
      
      window[[i]] <- data.frame((wX[length(wX)] + windowStart*prevPkDist[i]):(wX[length(wX)] + windowExtnd*prevPkDist[i]),                      # Define the window initially as from (the previous peak + (peak to peak distance / 2)) 
                     deriv1[(wX[length(wX)] + windowStart*prevPkDist[i]):(wX[length(wX)] + windowExtnd*prevPkDist[i])])                         # to (previous peak + (peak to peak distance * 1.35))
      windowInflxY <- d1InflxY[which(d1InflxX > window[[i]][1, 1] & d1InflxX < window[[i]][length(window[[i]][, 1]), 1])]        
      windowInflxX <- d1InflxX[which(d1InflxX > window[[i]][1, 1] & d1InflxX < window[[i]][length(window[[i]][, 1]), 1])]                       # Identify which inflection points are within the window       
      threshold <- quantile(window[[i]][, 2], probs=c(0.95))                                                                                    # Define threshold                                 
      windowPks <- which(windowInflxY > threshold)                                                                                              # Identify peaks
      windowPksY <- windowInflxY[windowPks]
      windowPks <- windowInflxX[windowPks]     
      
      # plot(window[[i]], type = "l")                                                                                                           # For debugging purposes, the window can be plotted
      # points(windowPks, windowPksY, pch = 19)
      
      ############# Window Peak Confirmation Criteria: #############
                                                                                                                                                
      if(length(windowPks) > 2){                                                                                                                # If more than 2 peaks are identified within the window, this can be due to four reasons:
        if(max(windowPksY) > (m+(2*std)) |  max(windowPksY) > (mean(deriv1[windowPks[1]:(windowPks[1]                                           # 1. a window does not include a genuine peak, and there are multiple secondary ones of similar height 
                           + (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))]))){                           # 2. there is an artefact with more than two peaks
                                   wX[length(wX)+1] <- windowPks[which(windowPksY == max(windowPksY))]                                          # 3. there are significantly large secondary peaks that also exceed the threshold for identification
                                   lowPks <- windowPksY[order(windowPksY)[1:2]]                                                                 # 4. a genuine peak has multiple inflection points   
          if(lowPks[1] > m+(2*std) & lowPks[2] > m+(2*std)                                                                                      
             & (windowPks[3] - windowPks[1]) > (prevPkDist[i]/10)){                                                                             # Check if the maximum peak is above a threshold relative to the time series (no 1)    
               cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300), ', deriv1[', (wX[i-1]-100), ':', (wX[i-1]+300),    # If it is, check if both lower peaks also exceed the threshold, 
               '], type = "l") ,', 'wave', i, '+/- 2 removed because the non-max peaks were high')                                              # if so mark them as artefactual (no 2), if not assume they are secondary peaks (no 3)
               artefacts[length(artefacts) + c(1, 2, 3, 4, 5)] <- c(i-2, i-1, i, i+1, i+2)                                                      # An additional condition of the above is that the 'peaks' are not too close together so as to be inflection points (no 4)
          }
        }else{                                                                                                                                  
          windowExtnd <- windowExtnd + 0.5                                                                                                      # If no 1 is the case, extend the window to look for peaks again
          windowPks <- c()
        }
      }
      
      if(length(windowPks) == 2){                                                                                                               # If two peaks are identified, they should be confirmed as genuine:
        if(max(windowPksY) > (m+(2*std)) | max(windowPksY) > (mean(deriv1[windowPks[1]:(windowPks[1]                                            # Check if the maximum peak exceeds the global or local threshold 
           + (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))]))){                                           # (local defined relative to peak to peak distance)
          if((windowPks[2] - windowPks[1]) < (prevPkDist[i]/3)){                                                                                # If it does, check if the two peaks are close togetber in time 
            wX[length(wX)+1] <- windowPks[which(windowPksY == max(windowPksY))]                                                                 # (within 1/3rd of the peak to peak distance) 
            cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300),                                                       # If they are, mark the highest peak as artefactual
                ', deriv1[', (wX[i-1]-100), ':', (wX[i-1]+300), '], type = "l") ,', 'wave',
                i, '+/- 2 removed because two peaks found too close together') 
          }else{                                                                                                                                # If they are not, go through both peaks in turn to confirm if genuine:
            if((windowPksY[1] > (m+(2*std))) | windowPksY[1] > (mean(deriv1[windowPks[1]:(windowPks[1]                                          # Check if the first peak can be marked as genuine by:
              + (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))])) &                                        # 1. comparing to time series threshold 
              windowPksY[1] > (windowPksY[2]/2)){                                                                                               # 2. comparing to local threshold 
                wX[length(wX)+1] <- windowPks[1]                                                                                                # 3. comparing to height of the other peak (should exceed it's half maximum)
                if(windowPksY[2] > (m+(2*std)) | windowPksY[2] > (mean(deriv1[windowPks[1]:(windowPks[1]                                        # If it can, the second peak can be marked as genuine with the same criteria
                + (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))])) &                                      
                windowPksY[2] > (windowPksY[1]/2)){   
                  wX[length(wX)+1] <- windowPks[2]  
              }
            }else{                                                                                                                              # If the first peak is not genuine, check the second and identify only the 
              if(windowPksY[2] > (m+(2*std)) | windowPksY[2] > (mean(deriv1[windowPks[1]:(windowPks[1] +                                        # second peak as genuine if appropriate. 
                (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))])) & 
                windowPksY[2] > (windowPksY[1]/2)){
                wX[length(wX)+1] <- windowPks[2] 
              }
            }
          }
        }else{                                                                                                                                  # If the maximum peak did not exceed threshold, extend the window and look again. 
          windowExtnd <- windowExtnd + 0.5
          windowPks <- c()
        }   
      }
      
      if(length(windowPks) == 1){                                                                                                               # If one peak is identified, confirm it is genuine by: 
        if(windowPksY > (m+(2*std)) | windowPksY > (mean(deriv1[windowPks[1]:(windowPks[1] +                                                    # 1. comparing to time series threshold
          (3*prevPkDist[i]))]) + 2*std(deriv1[windowPks[1]:(windowPks[1] + (3*prevPkDist[i]))])) |                                              # 2. comparing to local threshold
          windowPksY > (predict(d1p, wX[i-1])*0.9)){                                                                                            # 3. comparing it to the height of the previous peak                               
            wX[length(wX)+1] <- windowPks 
        }else{
          if((i-1) %in% artefacts){                                                                                                             # If the above criteria are not met, check if the previous peak was artefactual
            wX[length(wX)+1] <- windowPks                                                                                           
          }else{                                                                                                                                # If it was not, extend the window
            windowExtnd <- windowExtnd + 0.5
            windowPks <- c()
          }
        }
      }
      
      #############Window Artefact Identification: #############
      
      if(windowExtnd > 2 & is.na(printed)){                                                                                                     # If the window has been extended more than twice to find a peak, 
        cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300), ', deriv1[',                                              # it is likely that that peak may be an artefact; label it as such
           (wX[i-1]-100), ':', (wX[i-1]+300), '], type = "l") ,', 'wave', i, '+/- 2 removed because window needed extending')  
        printed <- 1 
        artefacts[length(artefacts) + c(1, 2, 3, 4, 5)] <- c(i-2, i-1, i, i+1, i+2)                                                             # i-1 and subsequent adjustments are used because wX[i] does not yet exist
      }
      
      if(min(window[[i]]) < (m-(3*std)) & is.na(printed)){                                                                                      # If a window contains a value that drops considerably below the mean, 
        cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300), ', deriv1[',                                              # label the peak found in that window as an artefact 
           (wX[i-1]-100), ':', (wX[i-1]+300), '], type = "l") ,', 'wave', i, '+/- 2 removed because the window contains a very low value') 
        artefacts[length(artefacts) + c(1, 2, 3, 4, 5)] <- c(i-2, i-1, i, i+1, i+2)
      }
      
      windowExtnd <- windowExtnd + 0.1                                                                                                          # If no peaks are found in a window (or only spurious ones found), 
    }                                                                                                                                           # extend the window and look again
  }       
  
  ############# Post Peak Identification Cleaning: #############
  
  d1wY <- predict(d1p, wX)                                                                                                                      # Find summary statistics of identified peak y-values
  m <- mean(d1wY) 
  std <- sd(d1wY) 
  if(length(artefacts) > 0){                                                                                                                    # Remove identified artefacts (should this be before calculating mean??)
    wX <- wX[-artefacts]
    d1wY <- d1wY[-artefacts]
  }
  
  artefacts <- c()                                                                                                                              # Check for unusually tall peaks, and label them as artefacts if:
  for(i in 2:(length(wX)-1)){     
    if(d1wY[i] > 1.5*d1wY[i+1] & d1wY[i] > 1.5*d1wY[i-1] | d1wY[i] > (m+(5*std))){                                                              # 1. the ith wave height exceeds local (comparison with i-1 and i+1) or time series thresholds
      cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300), ', deriv1[',
          (wX[i-1]-100), ':', (wX[i-1]+300), '], type = "l") ,', 'wave', i, '+/- 2 removed because of a very tall peak')
      artefacts[length(artefacts) + c(1, 2, 3, 4, 5)] <- c(i-2, i-1, i, i+1, i+2)
    }
    if(i > 5 & i < (length(wX)-5)){
      if(d1wY[i] > (1.5*mean(d1wY[c((i-5):(i+5))]))){
        cat('\n','Potential artefact',  ', plot(', (wX[i-1]-100), ':', (wX[i-1]+300), ', deriv1[',                                              # 2. the ith wave height exceeds another local threshold (derived from i-5 to i+5)  
          (wX[i-1]-100), ':', (wX[i-1]+300), '], type = "l") ,', 'wave', i, '+/- 2 removed because of a very tall peak')                        
        artefacts[length(artefacts) + c(1, 2, 3, 4, 5)] <- c(i-2, i-1, i, i+1, i+2)
      }
    }
  }       
  
  ############# Output: #############
  
  wY <- predict(sp, wX)                                    # Find W y_values on original trace
  Ws <- data.frame(wX, wY, d1wY)                           # Create output dataframe 
  if(length(artefacts) > 0){                               # Remove any further artefacts identified
    Ws <- Ws[-artefacts, ]
  }
  Ws <- Ws[-1, ]                                           # Remove the first and last waves
  Ws <-Ws[-nrow(Ws), ]
  
  colnames(Ws) <- c("wX", "wY", "wYD1")                    # Outputs as described above
  return(Ws)
}


find_u_v <- function(wx, wy, d1, d1p, spline, sr = samplingRate, plot = FALSE){
  ########################################################################################################################################
  # FindUV identifies the points on the systolic upstroke that correspond to the two half maximum values on the corresponding first  
  # derivative peak. The first, before w, is denoted U. The second, after w, is denoted V. 
  
  # Inputs: 
  # wx (A vector of x-coordinates corresponding to w points on the PPG time series)
  # wy (A vector of y-coordinates corresponding to w points on the PPG time series)
  # d1 (The first derivative time series, discrete form)
  # d1p (The first derivative time series, polynomial spline form)
  # spline (The original PPG time series, polynomial spline form)
  # sr (samplingRate)
  # plot (Logical, will plot the first derivative time series with U and V values as points if set to true)
  
  # Outputs:
  # uX (A vector of x-xoordinates corresponding to U points on the PPG time series)
  # uY (A vector of y-xoordinates corresponding to U points      "       "        )
  # vX (A vector of x-xoordinates corresponding to V points      "       "        )
  # vY (A vector of y-xoordinates corresponding to V points      "       "        )
  ########################################################################################################################################
                                        
  wHalfHeight <- predict(d1p, wx)/2                                                           # For each w point, identify the half maximum on first derivative
  halfHeightX <- c()                                                                          
  halfHeightY <- c()
  for(i in 1:length(wHalfHeight)){                                                            # For each 1st derivative peak, create a polynomial spline of the peak only
    d1PeakSub <- CubicInterpSplineAsPiecePoly((round(wx[i])-(sr/8)):(round(wx[i])+(sr/8)),
                 d1[(round(wx[i])-(sr/8)):(round(wx[i])+(sr/8))], "natural")                            
    preHalfHeights <- solve(d1PeakSub, b = wHalfHeight[i])                                    # Identify the x-coordinates of the new spline when the y-value = the half maximum
    
    if(length(preHalfHeights) < 2){                                                           # If only one coordinate is found, extend the length of the spline, 
      d1PeakSub <- CubicInterpSplineAsPiecePoly((round(wx[i])-(sr/4)):(round(wx[i])+(sr/4)),          # and search again
                   d1[(round(wx[i])-(sr/4)):(round(wx[i])+(sr/4))], "natural")                        
      preHalfHeights <- solve(d1PeakSub, b = wHalfHeight[i]) 
    }
    
    if(length(preHalfHeights) > 2){                                                           # More than one x-coordinate is sometimes found if the gradient of the upstroke 
      a <- preHalfHeights - wx[i]                                                             # is particularly variable
      b <- c()                                                                                # In such cases, find the time (x-axis) difference between each detected half height and w
      for(j in 1:(length(a)-1)){                                                              # Select the two values with the most similar difference before and after w
        b[j] <- a[j] - a[j+1]
      } 
      b[length(b) + 1] <- a[1] - a[length(a)]
      c <- which(abs(b) == min(abs(b)))
      if(c == length(b)){
        preHalfHeights <- c(preHalfHeights[c], preHalfHeights[1])
      }else{
        preHalfHeights <- c(preHalfHeights[c], preHalfHeights[c+1])
      }
    }
    halfHeightX[c((2*(i)-1), (2*(i)))] <- preHalfHeights                                      # Add the two values per peak to a vector of all in the time series
    halfHeightY[c((2*(i)-1), (2*(i)))] <- predict(d1PeakSub,                                  # And identify the corresponding y-coordinates for the (first derivative) times series 
                                          halfHeightX[c((2*(i)-1), (2*(i)))])  
  }
  
  if(plot){                                                                                   # Check results
    plot(d1p)
    points(halfHeightX, halfHeightY, pch = 19)
  }
  
  uX <- halfHeightX[seq_along(halfHeightX) %%2 != 0]                                          # Split the time series values into vectors for U and V
  vX <- halfHeightX[seq_along(halfHeightX) %%2 == 0]
  
  uY <- c()                                                                                   # Find the y-coordinates for U and V on the original PPG time series
  for(i in 1:length(uX)){
    uY[i] <- predict(spline, uX[i])
  }
  vY <- c()                                                                 
  for(i in 1:length(vX)){
    vY[i] <- predict(spline, vX[i])
  }
  
  df <- data.frame(uX, uY, vX, vY)                                                            # Organize outputs (see summary box above)
  return(df)
}


find_o <- function(wx, inx, iny, d1p, sp){
  ########################################################################################################################################
  # FindO identifies the origin of the systolic peak for each beat. In the absence of a clear inflection point at the origin, O points are
  # derived from inflection points in the first derivative. 
  
  # Inputs: 
  # wx (A vector of x-coordinates corresponding to w points on the PPG time series)
  # inx (A vector of all inflection point x-coordinates in the PPG time series)
  # iny (A vector of all inflection point y-coordinates in the PPG time series)
  # d1p (The first derivative time series, polynomial spline form)
  # sp (The original PPG time series, polynomial spline form)
  
  # Outputs:
  # o (vector of O points)
  # inx (vector of updated inflection point x-coordinates)
  # iny (vector of updated inflection point y-coordinates)
  ########################################################################################################################################
  
  o <- c()                                                                                    # Create vector to store O values
  for(i in 1:length(wx)){
    o[i] <- max(which(inx < wx[i]))                                                           # Identify O points as the those inflection points that 
  }                                                                                           # immediately precede w points. 

  inflexD1 <- solve(d1p, b = 0, deriv = 1)                                                    # Find inflection points on the first derivative
  inflexD1y <- predict(d1p, inflexD1)                                                         
  o2 <- c()                                                                                   # For each beat, find the inflection point before w that is 
  for(i in 1:length(wx)){                                                                     # also below 0 (points above 0 tend to be spurious)
    o2[i] <- max(which(inflexD1 < wx[i] & inflexD1y < 0))
  }
  
  for(i in 1:length(wx)){                                                                     # For each beat, replace the originally identified O with 
    if((inx[o][i] - inflexD1[o2][i]) < 0){                                                    # the O derived from the first derivative if the latter 
      inx[o][i] <- inflexD1[o2][i]                                                            # is closer to w in time (this occurs when there is no 
      iny[o][i] <- predict(sp, inx[o][i])                                                     # inflection point at the origin of the original PPG signal)
      }
  }
  
  tmp <- list(inx, iny, o)                                                                    # return the o points, as well as the inflection points 
  return(tmp)                                                                                 # which have been repositioned due to the above replacement
}


preclean_wuv <- function(w, uv, o, samp, sp, q = FALSE){
  ########################################################################################################################################
  # PreClean takes u, v and w values and assesses the x-axis (time) difference between them for each beat. For normal beats, time from
  # u to w is around half (50%) of the time from u to v. Beats with abnormal / artefactual systolic upstrokes tend to have outlying 
  # values for this measure. Thus PreClean can identify and remove them. 
  
  # Inputs: 
  # w (vector of all x-axis coordinates corresponding to w points)
  # uv (a dataframe containing x and y coordinates for all u and v values)
  # o (vector of O points)
  # samp (sample rate)
  # sp (The original PPG time series, discrete form)
  # q (Logical, will pause function and give the option of plotting rejected beats)
  
  # Outputs:
  # w (as above, with aberrant beats removed)
  # uv (              "       "             )
  # o (               "       "             )
  ########################################################################################################################################
  
  vDist <- c()                                                                                # For each wave, find the timing of w relative  
  for(i in 1:length(w$wX)){                                                                   # to u and v, expressed as a percentage 
    vDist[i] <- (w$wX[i] - uv$uX[i]) / (uv$vX[i] - uv$uX[i])*100
  }
  # plot(w$wX, vDist, type = "l")                                                             # This value may be of interest in itself (see plot)
  
  sdpdtv <- sd(vDist)                                                                         # Make a vector of all beats where this value is abnormal, 
  pdtvWaves <- c(which(vDist > (sdpdtv + median(vDist)) & vDist > 70),                        # either by being greater than one standard deviation 
                 which(vDist < (median(vDist) - sdpdtv) & vDist < 30))                        # from the median, or having an absolute value < 30 or > 70
  
  if(length(pdtvWaves) > 0){
    cat("\n", length(pdtvWaves), 
        "waves removed due to abnormal distances between u, v and w" )                        
    if(q == TRUE){                                                                            # If q = T, enter debug / plot routine
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(pdtvWaves)){
            if(pdtvWaves[i] == 1){
              plot(1:(samp*10), sp[1:(samp*10)], type = "l")
              points(w$wX, w$wY)
              points(w$wX[pdtvWaves[i]], w$wY[pdtvWaves[i]], pch =19, col = 'red')
              points(uv$uX, uv$uY)
              points(uv$vX, uv$vY)
            }else{
              plot((w$wX[pdtvWaves[i]]-samp*5):(w$wX[pdtvWaves[i]]+samp*5), 
                   sp[(w$wX[pdtvWaves[i]]-samp*5):(w$wX[pdtvWaves[i]]+samp*5)], type = "l")
              points(w$wX, w$wY)
              points(w$wX[pdtvWaves[i]], w$wY[pdtvWaves[i]], pch =19, col = 'red')
              points(uv$uX, uv$uY)
              points(uv$vX, uv$vY)
            }
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    w <- w[-pdtvWaves, ]                                                                      # Remove beats with abnormal values from all relevant vectors
    uv <- uv[-pdtvWaves, ]
    o <- o[-pdtvWaves]       
  }
  return(list(w, uv, o))
}


baseline <- function(inx, iny, o, dat, sp, plot = FALSE){
  ########################################################################################################################################
  # RemoveBaselineDrift generates a spline to fit the wandering baseline of the PPG time series and subtracts it from the time series. 
  # Given baseline drift is indicative of vasomotion and respiratory modulation, it may be of interest to extract the spline.
  
  # Inputs: 
  # inx (A vector of all inflection point x-coordinates in the PPG time series)
  # iny (A vector of all inflection point y-coordinates in the PPG time series)
  # o (A vector of all O points)
  # dat (The original PPG time series (preprocessed))
  # sp (The original PPG time series, polynomial spline form)
  # plot (Logical, plots baseline spline and the original PPG time series)
  
  # Outputs:
  # baseCor (time series with baseline wander removed)
  ########################################################################################################################################

  sfunction2 <- splinefun(inx[o], iny[o], method = "natural")                                 # Create a spline from the O points through
  splineBase <- sfunction2(seq(1, length(dat)), deriv = 0)                                    # the time series
  
  if(plot){                                                                                   # Plot the baseline spline and time series
    plot(sp)
    points(inx[o], iny[o], pch = 19)
    lines(splineBase)
  }
  
  baseCor <- dat - splineBase                                                                 # Subtract the baseline spline from the time 
  if(plot){                                                                                   # series, thereby removing baseline drift
    plot(baseCor, type = "l")                                                                  
    lines(1:length(dat), seq(from = 0, to = 0, length.out = length(dat)))                     # Plot new baseline (y = 0)     
  }
  
  return(baseCor)
}



clean_wuv <- function(wuv, sp, inx, o, samp, bc, q = FALSE){
  ########################################################################################################################################
  # CleanWuv first identifies erroneous beats and removes them, by identifying abnormal values of U and V, and the y-axis difference 
  # between them. It then calculates O-O intervals and inter-beat intervals, and removes beats preceding large intervals. 

  # Inputs: 
  # wuv (a dataframe of all w, u and v values corresponding to detected and pre-cleaned beats in the time series)
  # sp (the baseline-corrected PPG time series, polynomial spline form)
  # inx (a vector of all inflection point x-coordinates in the PPG time series)
  # o (a vector of all O points)
  # samp (sampling rate)
  # bc (the baseline-corrected PPG time series, discrete form)
  # q (logical, will pause function and give the option of plotting rejected beats)
  
  # Outputs:
  # d (a dataframe combining the following three structures):
    # wuv (the inputted dataframe of w, u and v values, with rows corresponding to erroneous beats removed)
    # diffVU (a vector of y-axis differences between u and v for each beat - for scaling purposes)
    # o2 (a vector of O points, with those corresponding to erroneous beats removed)
  # ibi (a vector of all inter-beat intervals (x-axis differences between sucessive W points))
  # oDiff (a vector of all O-O intervals (x-axis differences between successive O points))
  ########################################################################################################################################
  
  o2 <- o                                                                                     # Define a second vector for O from which 
                                                                                              # rejected waves are to be removed
  
  falseU <- c()                                                                               # Identify U values that are implausibly far 
  for(i in 1:(nrow(wuv)-1)){                                                                  # from baseline    
    if(wuv$uY[i] > (median(wuv$uY) + 2*std(wuv$uY)) & wuv$uY[i] > 1 &                         
       wuv$uY[i] > wuv$uY[i+1]*2 | wuv$uY[i] < -50){
      falseU[i] <- i
    }
  }
  if(length(falseU) > 0){
    falseU <- falseU[!is.na(falseU)]
    cat("\n", length(falseU), "/", nrow(wuv), 
        "waves removed due to U having an abnormally high y-value relative to baseline")
    if(q == TRUE){                                                                            # Optional debug mode for plotting waves  
      plotyyy <- 0                                                                            # with erroneous U values
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(falseU)){
            plot((wuv$wX[falseU[i]]-samp*2):(wuv$wX[falseU[i]]+samp*2), 
                 bc[(wuv$wX[falseU[i]]-samp*2):(wuv$wX[falseU[i]]+samp*2)], type = "l")
            points(wuv$uX[falseU[i]], wuv$uY[falseU[i]], pch = 19)
            points(wuv$uX[falseU[i]-1], wuv$uY[falseU[i]-1])
            points(wuv$uX[falseU[i]+1], wuv$uY[falseU[i]+1])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    if(length(falseU) > 0){
      o2 <- o2[-falseU]                                                                       # Remove erroneous U values
      wuv <- wuv[-falseU, ]
    }
  }
  
  diffVU <- wuv$vY - wuv$uY                                                                   # Find y-axis v-u difference (scale factor) for each wave 
  if(length(which(diffVU == 0)) > 0){                                                         # Remove waves where the above difference is 
    dup <- which(diffVU == 0)                                                                 # 0 (indicating an artefact)
    wuv <- wuv[-dup, ]
    diffVU <- diffVU[-dup]
    cat(length(which(diffVU == 0)), "/", nrow(wuv), 
        "waves removed due to scale factors of 0 (u and v incorrectly identified)")
  }
  
  falseScale <- which(diffVU < (median(diffVU) - 5*(std(diffVU))))                            # Identify beats where the v-u difference is abnormally small  
  if(length(falseScale) > 0){                                                                 # and remove them (with optional debug mode as above)
    cat("/n", length(falseScale), "/", nrow(wuv), 
        "waves removed due to scale factors of 0 (u and v incorrectly identified)")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(falseScale)){
            plot((wuv$wX[falseScale[i]]-samp*2):(wuv$wX[falseScale[i]]+samp*2), 
               bc[(wuv$wX[falseScale[i]]-samp*2):(wuv$wX[falseScale[i]]+samp*2)], type = "l")
            points(wuv$uX[falseScale[i]], wuv$uY[falseScale[i]])
            points(wuv$vX[falseScale[i]], wuv$vY[falseScale[i]])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    if(length(falseScale) > 0){
      o2 <- o2[-falseScale]
      wuv <- wuv[-falseScale, ]
      diffVU <- diffVU[-falseScale]
    }
  }
  
  oY <- predict(sp, inx[o])                                                                   # Identify O y-values on the original PPG trace
  owDiff <- c()                                                                               # Find the x-axis difference between O and W
  for(i in 1:length(wuv$wX)){
    owDiff[i] <- wuv$wX[i] - inx[o[i]]
  }
  
  oDiff <- c()                                                                                # Find the x-axis difference between successive o_points
  for(i in 1:(length(inx[o])-1)){
    oDiff[i] <- inx[o[i+1]] - inx[o[i]]
  }
  
  ibi <- c()                                                                                  # Find the x-axis difference between successive W points,  
  for(i in 1:(length(wuv$wX))-1){                                                             # and designate as inter-beat intervals
    ibi[i] <- wuv$wX[i+1] - wuv$wX[i]
  }
  
  o2 <- o2[-length(o2)]                                                                       # Remove the last W (in case final beat is incomplete)
  wuv <- wuv[-nrow(wuv), ]
  diffVU <- diffVU[-length(diffVU)]  
                                                                                                      
  # plot(ibi)                                                                                 # Identify abnormal inter-beat intervals indicative of 
  # points(which(ibi > 1.25*median(ibi)),                                                     # an extended time between beats (e.g due to artefact)
  # ibi[which(ibi > 1.25*median(ibi))], pch = 19, col = "red")                                # These can be plotted (see left)
  
  endWaves <- c()                                                                             # Abnormal inter-beat intervals are defined as greater 
  for(i in 2:(length(ibi)-1)){                                                                # than the median interval by a factor of 1.3, and
    if(ibi[i] > 1.3*median(ibi) & ( ibi[i] > 2*ibi[i-1] | ibi[i] > 2*ibi[i+1])){              # greater than at least one of the values adjacent
      endWaves[i] <- i                                                                        # to it by a factor of 2
    }                                                                                         
  }                                                                                           # Beats preceding abnormal intervals are removed as they
  if(length(endWaves) > 0){                                                                   # will not have an O point to mark their end
    endWaves <- endWaves[!is.na(endWaves)]
    cat("\n", length(endWaves), "/", nrow(wuv), 
        "waves removed due to the end of the wave (next o point) 
         not being corrected for baseline")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(endWaves)){
            if(endWaves[i] == 1){
              plot(1:(samp*10), bc[1:(samp*10)], type = "l")
              points(wuv$wX[endWaves[i]], wuv$wY[endWaves[i]], pch =19, col = 'red')
            }else{
              plot((wuv$wX[endWaves[i]]-samp*5):(wuv$wX[endWaves[i]]+samp*5), 
                   bc[(wuv$wX[endWaves[i]]-samp*5):(wuv$wX[endWaves[i]]+samp*5)], type = "l")
              points(wuv$wX[endWaves[i]], wuv$wY[endWaves[i]], pch =19, col = 'red')
              points(wuv$wX[endWaves[i]+1], wuv$wY[endWaves[i]+1], pch = 19, col = 'red')
              points(wuv$wX, wuv$wY)
            }
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    if(length(endWaves) > 0){
      o2 <- o2[-endWaves]
      wuv <- wuv[-endWaves, ]
      diffVU <- diffVU[-endWaves] 
    }
  }
  d <- cbind(wuv, diffVU, o2)                                                                 # Bind vectors with element number corresponding to beat number 
  dat <- list(d, ibi, oDiff)                                                                  # into a single dataframe before outputting 
  return(dat)
}


sep_beats <- function(odiff, bc, samp, wuv, wvlen, inx, o, ibi, scale = TRUE, q = FALSE, subset = FALSE, boundaries = NULL){   
  
  # Optional subsetting of Iso Waves:
  if(subset == T){
    #Find the Iso specific part of the data
    ab_ibi <- which(ibi > mean(ibi) + (4*sd(ibi)) | ibi < mean(ibi) - (4*sd(ibi))) #excluding ibi points 4 sds above mean
    ibi[ab_ibi] <- NA 
    ibi <- ibi[!is.na(ibi)]
    meds <- rollmedian(ibi, k = 19) #rolling median
    basemed <- mean(meds[1:50]) #baseline defined as the average of the first 50 rolling median points... I'm not sure if this is sensible
    halfs <- (basemed - min(meds))/2 #half diff between minimum and baseline
    post <- min(meds) + halfs   # the half way point (the y axis point that represents the half height)
    # Sometimes the minimum is incorrect (due to outlier ibi values):
    # Suggest first finding the minimum of meds and then finding a minimum ibi local to that (+/- 10 beats):
    meds_roi <- (which(meds == min(meds))[1] - 10) : (which(meds == min(meds))[1] + 10)
    # If no discernable minimum in the time series, it is possible a value far from the middle would be included, which would introduce negative values to meds_roi
    if( sum(which(meds_roi < 1)) > 0  | sum(which(meds_roi > length(ibi))) > 0 ){
      message("Warning: No discernable minimum in 2mg trace")
      Sys.sleep(10)
      meds_roi <- (round(length(ibi)/2) - 10) : (round(length(ibi)/2) + 10)
    }
    indx <- meds_roi[which(ibi[meds_roi] == min(ibi[meds_roi]))]    # indx is taken as the minimum
    # Check again that minimum is not too near to beginning / end of trace:
    if(indx < 15 | indx > (length(ibi) - 15)){
      message("Warning: No discernable minimum in 2mg trace")
      Sys.sleep(10)
      meds_roi <- (round(length(ibi)/2) - 10) : (round(length(ibi)/2) + 10)
    }
    pre_indx <- which(abs(ibi[1:indx] - post) < 0.5) + 0 # finds the values that are closest to the half point  # To ensure pre_indx is not < 75, have change ibi[1:indx] to ibi[75:indx], and 0 to 74...
    # If no values within 0.5 of the half-way point, extend the range to 1.5 and so on:
    a <- 0.5
    while(length(pre_indx) < 1){
      a <- a + 1
      pre_indx <- which(abs(ibi[1:indx] - post) < a) + 0 # Have to adjust for the fact that 1 = 75 now
    }
    in2 <- which(abs(pre_indx - indx) == min(abs(pre_indx - indx))) # take the one closest to the minimum
    pre_indx <- pre_indx[in2]
    
    # Find the closest to the half point from the points after the minimum
    post_indx <- indx + which(abs(ibi[indx:length(ibi)] - post) < 0.5)   # changed length(ibi) to 210
    a <- 0.5
    while(length(post_indx) < 1){
      a <- a + 0.25
      post_indx <- indx + which(abs(ibi[indx:length(ibi)] - post) < a)  # changed length(ibi) to 210
    }
    while(post_indx[1] < indx + 15){
      post_indx <- post_indx[-1]
    }
    post_indx <- post_indx[1]
    rm(meds, basemed, halfs, post, indx, in2)
    ppg_pre <- (which((ppg[,1]) == beat[,1][pre_indx - 1]))    # Before infusion
    ppg_post <- which((ppg[,1]) == beat[,1][post_indx + 1])    # After infusion wears off
    # Sometimes beat[, 1] will not contain the values specified to subset, in which case... 
    if(length(ppg_pre) < 1 | length(ppg_post) < 1){
      message("Subsetting failed: review of time series suggested")
      Sys.sleep(10)
      ppg_pre <- which((ppg[,1]) == beat[1,1])
      ppg_post <- which((ppg[,1]) == beat[nrow(beat),1])
    }
    
    # You might have to re-derive pre and post indx from ppg pre and post
    subs <- which(wuv$wX > ppg_pre & wuv$wX < ppg_post)
    cat(length(subs), "beats in subset (pre-cleaning)")
    #Sys.sleep(2)
    
    # Subset:
    wuv <- wuv[subs, ]
    # Subset ibi:
    ibi <- ibi[subs]
  }
  
  if(subset == "rep"){
    
    # Subset according to previous time series:
    subs <- which(wuv$wX > boundaries[1])
    subs_pre <- subs[1:boundaries[2]]
    # There's a chance there aren't enough beats remaining in the placebo time series, in which case take as many as possible:
    tmp <- sum(is.na(subs_pre))
    if(tmp > 0){
      subs <- subs[1:(length(subs_pre)-tmp)]
      boundaries[2] <- length(subs)
    }else{
      subs <- subs[1:boundaries[2]]
    }
    
    # Subset:
    wuv <- wuv[subs, ]
    # Subset:
    ibi <- ibi[subs]
  }
  
  
  # Redefine baseline corrected data:
  sourcedata <- bc[1:length(undetrended)]
  
  # Define a dataframe to contain individual waves (first column is the x-axis (in seconds) - currently set for bioradio data):
  pulse <- data.frame(seq((-141/(samp*10)), ((wvlen*15 -9)-142)/(samp*10), by = 1/(samp*10)))   
  
  afterO <- list()
  beforeO <- list()
  extra_long_wave <- c()
  for(i in 1:length(wuv$wX)){  

    splPolySub <- CubicInterpSplineAsPiecePoly((round(wuv$uX[i])-15):(round(wuv$uX[i]) + (wvlen+10)), sourcedata[(round(wuv$uX[i])-15):(round(wuv$uX[i]) + (wvlen+10))], "natural")
    # Turn into discrete form
    splSub <- predict(splPolySub, c(seq((wuv$uX[i]-14), (wuv$uX[i]+(wvlen+5)), 0.1)))
    # Make into dataframe:
    splSub <-  as.data.frame(splSub)
    splSub <- cbind(splSub, c(seq((wuv$uX[i]-14), (wuv$uX[i]+(wvlen+5)), 0.1)))
    colnames(splSub) <- c('y', 'x')  
    # Scale so that v-u = 1
    if(scale == TRUE){
      splSub$y <- splSub$y/(wuv$diffVU[i])      
    }
    # Adjust such that u = 0, v = 1 on y-axis
    yDiff <- splSub$y[141] 
    splSub$y <- splSub$y - yDiff
    
    if(scale == TRUE){
      # Find the x-value for each wave that corresponds to when it = 0.5 in height (this requires making a spline):
      splPolySub2 <- CubicInterpSplineAsPiecePoly(splSub$x, splSub$y, "natural")
      halfCross <- solve(splPolySub2, b = 0.5, deriv = 0)
      halfCross <- halfCross[which(abs(halfCross - wuv$wX[i]) == min(abs(halfCross - wuv$wX[i])))]    
      
      # Convert to discrete form again: (need to redefine splSub)
      splSub2 <- predict(splPolySub, c(seq((halfCross-14), (halfCross+(wvlen+5)), 0.1)))  
      splSub2 <-  as.data.frame(splSub2)
      splSub2 <- cbind(splSub2, c(seq((halfCross-14), (halfCross+(wvlen+5)), 0.1)))  
      colnames(splSub2) <- c('y', 'x') 
      
      # Scale again
      splSub2$y <- splSub2$y/(wuv$diffVU[i]) 
      # Adjust y-axis such that u = 0, v = 1
      yDiff <- wuv$uY[i] / wuv$diffVU[i]
      splSub2$y <- splSub2$y - yDiff
    }else{
      splSub2 <- splSub
    }
    
    # Find next_o
    afterO[[i]] <- which(splSub2$x > inx[o][min(which(inx[o] > wuv$wX[i]))])
    
    # Find values before the o of the wave itself 
    beforeO[[i]] <- which(splSub2$x < inx[o][max(which(inx[o] < wuv$wX[i]))])
    
    # Correct such that x column and wave column are correctly aligned
    splSub3 <- c()
    for(j in 1:nrow(splSub2)){
      splSub3[j+1] <- splSub2$y[j]
    }
    
    # If splSub3 and nrow(pulse) are the same length, you need only adjust afterO
    if(length(splSub3) == nrow(pulse)){
      if(length(afterO[[i]]) > 0){
        diff2 <- abs(length(splSub3) - max(afterO[[i]]))
        for(j in 1:diff2){
          afterO[[i]] <- c(afterO[[i]], (max(afterO[[i]]) + 1))
        }
      }
    }
    
    # Or
    # Adjust such that splSub3 is the same length as pulse
    if(length(splSub3) > nrow(pulse)){
      diff <- length(splSub3) - nrow(pulse)
      len <- length(splSub3)
      splSub3 <- splSub3[-((len - (diff-1)):len)]
      if(diff > 1){     # must correct the afterO values so that they also do not contain values beyond the length of splSub3 (include case where length of afterO[[i]] is one so the code works...)
        if(length(afterO[[i]]) > 1 ){
          afterO[[i]] <- afterO[[i]][-(which(afterO[[i]] > length(splSub3)))]  #afterO[[i]][1:(which(afterO[[i]] == (len - (diff-1))) - 1) 
        }else{
          afterO[[i]] <- afterO[[i]][-(which(afterO[[i]] > length(splSub3)))]
        }
      }
    }
    
    
    if(length(splSub3) < nrow(pulse)){
      diff <- nrow(pulse) - length(splSub3)  # diff = how much longer pulse is than splSub3...
      splSub3 <- c(splSub3, rep(NA, diff))   # The extra rows in pulse are thus made up by NAs
      if(length(afterO[[i]]) > 0){
        diff2 <- length(splSub3) - max(afterO[[i]])   # This = how many elements at the end of splSub3 are after O.
        # This for loop adds values to after_o to make up the difference
        for(j in 1:diff2){
          afterO[[i]] <- c(afterO[[i]], (max(afterO[[i]]) + 1))
        }
      }
    }
    
    # Add column to dataframe
    pulse <- cbind(pulse, splSub3)
  }
  
  for(i in 1:(ncol(pulse) -1)){ 
    colnames(pulse)[i+1] <- paste("wave", i, sep = "_")       
  }
  colnames(pulse)[1] <- "x"
  
  # Remove any values after O for each wave:
  for(i in 2:(ncol(pulse))){
    pulse[, i][afterO[[(i-1)]][-1]] <- NA  
  }
  
  # Remove values before O before each wave:
  for(i in 2:(ncol(pulse))){
    pulse[, i][beforeO[[(i-1)]][-1]] <- NA  
  }

  # Remove any extra long waves (i.e where a distance of 2 Os has been counted as one wave):
  # find average length of waves (without NAs) in pulse
  wavelengths <- c()
  for(i in 2:ncol(pulse)){
    wavelengths[i] <- length(pulse[, i][!is.na(pulse[, i])])  
  }
  wavelengths <- wavelengths[!is.na(wavelengths)]
  
  
  ########## Cleaning: #########
  
  extra_long_wave <- c()
  for(i in 2:length(wavelengths)){
    if(wavelengths[i] > (mean(wavelengths) + sd(wavelengths)) & wavelengths[i] > 1.8*(wavelengths[i-1]) ){
      extra_long_wave[i] <- i
    }
  }
    extra_long_wave <- extra_long_wave[!is.na(extra_long_wave)]
    if(length(extra_long_wave) > 0){
      #rejected_waves <- rejected_waves
      cat("\n", length(extra_long_wave), "/", (ncol(pulse)-1), "waves removed for being abnormally long")
      if(q == TRUE){
        plotyyy <- 0
        while(plotyyy == 0){
          plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
          if(plotyy == "yes"){
            for(i in 1:length(extra_long_wave)){
              plot((wuv$wX[extra_long_wave[i]]-samp*2):(wuv$wX[extra_long_wave[i]]+samp*2), bc[(wuv$wX[extra_long_wave[i]]-samp*2):(wuv$wX[extra_long_wave[i]]+samp*2)], type = "l")
              points(wuv$wX[extra_long_wave[i]], wuv$wY[extra_long_wave[i]], pch =19, col = 'red')
            }
            plotyyy <- 1
          }
          if(plotyy == "no"){
            cat("\n", "ok") 
            plotyyy <- 1
          }
          if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
        }
      }
      wuv <- wuv[-(extra_long_wave[!is.na(extra_long_wave)]-1), ]
      pulse <- pulse[, -(extra_long_wave[!is.na(extra_long_wave)])]  
      ibi <- ibi[-(extra_long_wave[!is.na(extra_long_wave)]-1)]
    }
  

  
  extra_short_waves <- c()
  for(i in 2:length(wavelengths)){
    if(wavelengths[i] < mean(wavelengths[-1]) - 3*sd(wavelengths[-1])){
      extra_short_waves[i] <- i
    }
  }
  extra_short_waves <- extra_short_waves[!is.na(extra_short_waves)]
  if(length(extra_short_waves) > 0){
    cat("\n", length(extra_short_waves), "/", (ncol(pulse)-1), "waves removed for being abnormally short in duration")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(extra_short_waves)){
            plot((wuv$wX[extra_short_waves[i]]-samp*2):(wuv$wX[extra_short_waves[i]]+samp*2), bc[(wuv$wX[extra_short_waves[i]]-samp*2):(wuv$wX[extra_short_waves[i]]+samp*2)], type = "l")
            points(wuv$wX[extra_short_waves[i]], wuv$wY[extra_short_waves[i]], pch =19, col = 'red')
            points()
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(extra_short_waves-1), ]
    pulse <- pulse[, -(extra_short_waves)]  
    ibi <- ibi[-(extra_short_waves-1)]
  }

  
  double_segments <- c()
  double_peaks_wave <- list()
  for(i in 2:ncol(pulse)){
    # Make a spline to identify inflection points:
    wave <- pulse[, i][!is.na(pulse[, i])]
    sfunction <- splinefun(1:length(wave), wave, method = "natural")
    spline1 <- sfunction(seq(1, length(wave)), deriv = 0)
    splinePoly <- CubicInterpSplineAsPiecePoly(1:length(wave), spline1, "natural") 
    inflexX <- solve(splinePoly, b = 0, deriv = 1)
    inflexY <- predict(splinePoly, inflexX)
    peaks <- which(inflexY > mean(inflexY) + 1.7*sd(inflexY))
    
    # plot(splinePoly)
    # points(inflexX, inflexY)
    
    # If there are more than one inflection points above threshold and the x-axis distance between the first and last is greater than 100, assume a double segment
    if(length(peaks) > 0){
      if( length(peaks) > 1 & ((inflexX[peaks][length(inflexX[peaks])] - inflexX[peaks[1]]) > 100) ){
        double_segments[i] <- i 
        double_peaks_wave[[i]] <- wave
      }
    }
  }
  double_segments <- double_segments[!is.na(double_segments)]
  if(length(double_peaks_wave) > 1){double_peaks_wave <- double_peaks_wave[-(which(sapply(double_peaks_wave, is.null)))]}        
  if(length(double_segments) > 0){
    cat("\n", length(double_segments), "/", (ncol(pulse)-1), "waves removed for containing two systolic peaks")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(double_segments)){
            plot(1:length(double_peaks_wave[[i]]), double_peaks_wave[[i]], t = "l")
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(double_segments-1), ]
    pulse <- pulse[, -(double_segments)]  
    ibi <- ibi[-(double_segments-1)]
  }
  

  # Waves that enter a second systolic upstroke
  systolic_endings <- c()
  for(i in 2:ncol(pulse)){
    wave <- pulse[, i] 
    wave <- wave[!is.na(wave)]
    if( wave[length(wave)] > 0.25 | (length(wave) > mean(wavelengths) & (max(wave[round(0.75*length(wave)):length(wave)]) > 0.8)) ){            # Adding additional criterion here: if the wave is above average length and the last quarter has a value above 0.8, consider it also a second systolic peak
      systolic_endings[i] <- i
    }   
  }
  systolic_endings <- systolic_endings[!is.na(systolic_endings)]
  if(length(systolic_endings) > 0){
    cat("\n", length(systolic_endings), "/", (ncol(pulse)-1), "waves removed for including the following systolic upstroke")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(systolic_endings)){
            plot(pulse[, systolic_endings[i]])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(systolic_endings-1), ]
    pulse <- pulse[, -systolic_endings]
    ibi <- ibi[-(systolic_endings-1)]
  }
  
  # Find average wave:
  average_wave <- find_average(p = pulse, ao = afterO)
  
  # Waves that fall significantly below O:
  drops_below_o <- c()
  for(i in 2:ncol(pulse)){
    wave <- pulse[, i][!is.na(pulse[, i])]
    thd <- 4   # threshold subject to change (default 2)
    if((min(wave) < wave[1]*thd | min(wave) < -0.3 | min(wave) < min(average_wave[!is.na(average_wave)])*thd) & min(wave) < 0){  
      drops_below_o[i] <- i
    }
  }
  drops_below_o <- drops_below_o[!is.na(drops_below_o)]
  if(length(drops_below_o) > 0){
    cat("\n", length(drops_below_o), "/", (ncol(pulse)-1), "waves removed for having values significantly below baseline")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(drops_below_o)){
            plot(pulse[, drops_below_o[i]])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(drops_below_o-1), ]        # Remember, the pulse has the 1st column, meaning pulse[, drops_below_o] = wuv[drops_below_o - 1, ] (hence the -1 here) 
    pulse <- pulse[, -drops_below_o]    
    ibi <- ibi[-(drops_below_o-1)]
  }
  
  
  # Recalculate average after removing the most aberrant waves:
  average_wave <- find_average(p = pulse, ao = afterO)
  
  # Find the SD wave:
  sd_wave <- find_sd(p = pulse, ao = afterO)
  
  # Remove waves that have a high SD of residuals:
  resid_sd <- c()
  for(i in 2:ncol(pulse)){
    residuals <- average_wave[142:length(average_wave)] - pulse[, i][142:length(pulse[, i])]
    resid_sd[i] <- sd(residuals[!is.na(residuals)][-c(1:100)])  # removed 1:100 since at this point all values are close to the average on the systolic upstroke
  }
  resid_sd <- sqrt(resid_sd)  # equivalent of squaring, since the resifual values are fractions when scaled
  resid_sd <- resid_sd[-1] # remove the NA
  # thld <- mean(resid_sd) - sd(resid_sd)*0.5   # threshold could be adjusted (has been increased from 2)
  thld <-  0.35  
  hrsd_waves <- which(resid_sd > thld) + 1  # +1 to account for the removed NA value
  if(length(hrsd_waves) > 0){
    cat("\n", length(hrsd_waves), "/", (ncol(pulse)-1), "waves removed for having high residual SD")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(hrsd_waves)){
            plot(pulse[, hrsd_waves[i]])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(hrsd_waves-1), ]
    pulse <- pulse[, -hrsd_waves]
    ibi <- ibi[-(hrsd_waves-1)]
  }
  
  
  # Waves that are beyond 5 SDs away from the average:
  outlier_waves <- c()
  for(i in 2:ncol(pulse)){
    breaches <- c()
    wave <- pulse[, i]
    for(j in 142:length(wave)){   # look only after w
      if(is.na(pulse[j, i]) == FALSE & is.na(average_wave[j] + 4*sd_wave[j]) == FALSE){
        if(pulse[j, i] > (average_wave[j] + 4*sd_wave[j]) | pulse[j, i] < (average_wave[j] - 4*sd_wave[j])){
          breaches[j] <- 1
        }
      }
    }
    if(sum(breaches[!is.na(breaches)]) > 0){
      outlier_waves[i] <- i
    }
  }
  outlier_waves <- outlier_waves[!is.na(outlier_waves)]
  if(length(outlier_waves) > 0){
    cat("\n", length(outlier_waves), "/", (ncol(pulse)-1), "waves removed for having values beyond 5 SDs from the average")
    if(q == TRUE){
      plotyyy <- 0
      while(plotyyy == 0){
        plotyy <- readline(prompt = "Would you like to view? (enter yes or no)")
        if(plotyy == "yes"){
          for(i in 1:length(outlier_waves)){
            plot(pulse[, outlier_waves[i]])
          }
          plotyyy <- 1
        }
        if(plotyy == "no"){
          cat("\n", "ok") 
          plotyyy <- 1
        }
        if(plotyy != "yes" & plotyy != "no"){cat("\n", "please enter 'yes' or 'no'")}
      }
    }
    wuv <- wuv[-(outlier_waves-1), ]
    pulse <- pulse[, -outlier_waves]
    ibi <- ibi[-(outlier_waves-1)]
  }
  
  # Final recalculation of average wave:
  average_wave <- find_average(p = pulse, ao = afterO) 
  
  # print beats carried over:
  cat(nrow(wuv), "beats carried over for 2mg subsetting (post-cleaning)")
  
  # Save rejected waves:
  rejects <- list(extra_long_wave, extra_short_waves, double_segments, systolic_endings, drops_below_o, hrsd_waves, outlier_waves)
  
  dat <- list(average_wave, pulse, wuv, rejects)
  if(subset == T){dat <- list(average_wave, pulse, wuv, rejects, c(ppg_pre, nrow(wuv)), ibi)}
  
  return(dat)
}


find_average <- function(p, ao){
  
  # Find the last 10 values of each wave:
  last10 <- list()
  for(i in 2:(ncol(p))){
    last10[[i-1]] <- p[, i][!is.na(p[, i])][(length(p[, i][!is.na(p[, i])])-10):(length(p[, i][!is.na(p[, i])]))]
  }
  
  # Find the average last 10 values
  avLast10 <- c()
  for(i in 1:10){   
    rowVec <- c()
    for(j in 1:length(last10)){      
      rowVec[j] <- last10[[c(j, i)]] 
    }
    avLast10[i] <- mean(rowVec[!is.na(rowVec)])   
  }
  
  # Find the consecutive y-axis differences (gradient essentially) of last 10 values:
  avDiff <- c()
  for(i in 1:9){                    # 9 here since 10 values will give 9 values for differences between them
    avDiff[i] <- avLast10[i+1] - avLast10[i]
  }
  
  # Simply calculating the average wave by averaging each row of the dataframe doesn't work at the end of the wave, 
  # since as waves end, they no longer factor in to the calculation of the average. The average would be erroneously 
  # drawn out until the end of the last wave. Therefore, a clone dataframe of p is used to continue the trajectories
  # of waves after they have ended (by using the average gradient above) and thus make the end of the average wave
  # a more accurate approximation of the true average. 
  
  # Replace NA values with continuing downward gradient in a clone dataframe:
  p2 <- p
  for(i in 2:(ncol(p2))){
    for(j in 1:length(p2[, i][ao[[(i-1)]][-1]])){
      p2[, i][ao[[(i-1)]][-1]][j] <- p2[, i][ao[[(i-1)]][1]] + j*mean(avDiff[1:5])
    }
  }
  
  # Calculate the average wave by row:
  avWav <- c()
  sdWav <- c()
  medWav <- c()
  for(i in 1:nrow(p2)){
    rowVec <- c()
    for(j in 2:(ncol(p2))){
      rowVec[j-1] <- p2[i, j] 
    }
    avWav[i] <- mean(rowVec[!is.na(rowVec)])   
    sdWav[i] <- sd(rowVec[!is.na(rowVec)]) 
    medWav[i] <- median(rowVec[!is.na(rowVec)])
  }
  
  # Find where the end of the average wave should end: 
  # This is done by finding the average (mode) y-value of the final value of each wave (before trajectory continuation)
  end <- c()
  for(i in 2:ncol(p)){
    end[i-1] <- p2[, i][ao[[(i-1)]][1]]
  }
  # define mode function
  mode <- function(x) {
    ux <- unique(x)
    ux[which.max(tabulate(match(x, ux)))]
  }
  # Made this into a for loop because there can be waves that have no values after o, and if all waves are like that, end will be null
  if(length(end) > 1){     
    avEnd <- mode(round(end[!is.na(end)], digits = 2))
    # Remove elements of average wave after where its end should be:
    belowO <- which(avWav < avEnd)
    # Find the first point in the last quarter of the wave that goes below baseline
    if(sum(which(belowO > (length(avWav)/(4/3)))) > 0){
      b. <- belowO[min(which(belowO > (length(avWav)/(4/3))))]
    }else{
      b. <- NA
    }
    # If the wave doesn't go below baseline, don't remove any 
    if(is.na(b.) == FALSE){
      # otherwise, remove those elements from the average:
      avWav[b.:length(avWav)] <- NA
    }
  }
  
  # Find the median of the first x-values - make that the start of average:
  first <- c()
  for(i in 2:ncol(p)){
    stpqw <- p[, i][1:200]
    stq <- stpqw[is.na(stpqw)]
    first[i-1] <- length(stq) + 1
  }
  start <- median(first)-2
  avWav[1:start] <- NA
  
  # From the last value moving backwards, if it's below the value of the first avWave value, remove it... 
  avWave <- as.vector(avWav)
  
  avWave_tmp <- avWave[!is.na(avWave)]
  
  while(avWave_tmp[length(avWave_tmp)] < avWave_tmp[1]){
    avWave_tmp <- avWave_tmp[-length(avWave_tmp)]
    avWave[length(avWave_tmp) + which(!is.na(avWave))[1]] <- NA
  }
 
  return(avWave)
}



find_sd <- function(p, ao){
  
  # Find the last 10 values of each wave:
  last10 <- list()
  for(i in 2:(ncol(p))){
    last10[[i-1]] <- p[, i][!is.na(p[, i])][(length(p[, i][!is.na(p[, i])])-10):(length(p[, i][!is.na(p[, i])]))]
  }
  
  # Find the average last 10 values
  mean_last10 <- c()
  for(i in 1:10){   
    row_vector <- c()
    for(j in 1:length(last10)){      
      row_vector[j] <- last10[[c(j, i)]] 
    }
    mean_last10[i] <- mean(row_vector[!is.na(row_vector)])   
  }
  
  # Find the consecutive y-axis differences (gradient essentially) of last 10 values:
  mean_diff_last10 <- c()
  for(i in 1:9){                    # 9 here since 10 values will give 9 values for differences between them
    mean_diff_last10[i] <- mean_last10[i+1] - mean_last10[i]
  }
  
  # Replace NA values with continuing downward gradient in a clone dataframe:
  p_for_finding_average <- p
  for(i in 2:(ncol(p_for_finding_average))){
    for(j in 1:length(p_for_finding_average[, i][ao[[(i-1)]][-1]])){
      p_for_finding_average[, i][ao[[(i-1)]][-1]][j] <- p_for_finding_average[, i][ao[[(i-1)]][1]] + j*mean(mean_diff_last10[1:5])
    }
  }
  
  # Calculate the average wave by row:
  average_wave <- c()
  sd_wave <- c()
  median_wave <- c()
  for(i in 1:nrow(p_for_finding_average)){
    row_vector <- c()
    for(j in 2:(ncol(p_for_finding_average))){
      row_vector[j-1] <- p_for_finding_average[i, j] 
    }
    average_wave[i] <- mean(row_vector[!is.na(row_vector)])   
    sd_wave[i] <- sd(row_vector[!is.na(row_vector)]) 
    median_wave[i] <- median(row_vector[!is.na(row_vector)])
  }
  
  return(as.vector(sd_wave))
}



diast_pk <- function(avw, sr, scale = F, dias_param = NULL){
  # Find the diastolic peak on the average wave to inform OSND finding (also some adjusment of x-values for removal of NA values):
  avw <- avw[!is.na(avw)]
  
  # Need to find new W position (0.5) after removing NAs
  if(scale == TRUE){
    xShift <- which(abs(avw-0.5) == min(abs(avw - 0.5)))
  }else{
    xShift <- which.min(abs(avw)) 
  }
  avWavePoly <- CubicInterpSplineAsPiecePoly(1:length(avw), avw, "natural")
  avInflexX <- solve(avWavePoly, b = 0, deriv = 1)
  avInflexY <- predict(avWavePoly, avInflexX)
  
  # plot(avWavePoly)
  # points(avInflexX, avInflexY) 
  
  # Specify limitations for where the diastolic peak can first be found i.e between 120:230 on x-axis, and below 1 on y-axis:
  #peaks <- order(avInflexY[which(avInflexX < 215 & avInflexX > 120 & avInflexY < 1)], decreasing = TRUE)  
  #diastPk <- avInflexX[which(avInflexX < 215 & avInflexX > 120 & avInflexY < 1)][peaks[1]]
  
  # OR:
  # Find any peaks (above 0):
  peaks_above_0 <- which(avInflexY > 0)
  
  # If a fitted wave is being assessed, find out what the dias_param corresponds to:
  if(!is.null(dias_param)){
    # peaks_above_0[1] can either be O or S - make sure it is being assigned to S:
    if( avInflexY[peaks_above_0[1]] <  mean(avw) ){
      peaks_above_0 <- peaks_above_0[-1]
    }
    dias_param <- dias_param + avInflexX[peaks_above_0[1]]
    diastPk <-  avInflexX[peaks_above_0[which( abs(avInflexX[peaks_above_0] - dias_param) == min(abs(avInflexX[peaks_above_0] - dias_param)))]] 
    # If the value assigned is just the max of the wave (i.e systolic peak, default to sr*500):
    if(avInflexY[peaks_above_0[which( abs(avInflexX[peaks_above_0] - dias_param) == min(abs(avInflexX[peaks_above_0] - dias_param)))]] - max(avw) < 2){
      diastPk <- 5*sr
    }
    peaks <- peaks_above_0
    fitted <- 1
  }else{
    fitted <- 0
  }
  
  if(fitted != 1){
    # Order peaks above 0 in descending order
    peaks <- order(avInflexY[peaks_above_0], decreasing = TRUE)
    # This ensures peaks refers to the correct avInflexX points
    peaks <- peaks_above_0[peaks]
    # But reducing them all as required so that there is no unnamed amount of non-peaks at the beginning of the wave, might be neater
    # So make the first peak 1:
    dif <- peaks[1] - 1
    peaks <- peaks - dif
    # Correct avInflexX / avInflexY:
    if(dif > 0){
      avInflexX <- avInflexX[-c(1:dif)]
      avInflexY <- avInflexY[-c(1:dif)] 
    }
    # Remove 0 or negative values if they have resulted from the above subtraction:
    if(sum(peaks < 1) > 0){
      peaks <- peaks[-which(peaks < 1)]   
    }
    
    # Take the second highest inflection point as the diastolic peak (unless diastolic is taller than systolic, then then take the highest)
    if(peaks[1] != 1 & length(peaks) > 1){diastPk <- avInflexX[peaks[1]]}else{diastPk <- avInflexX[peaks[2]]}
    # unless it is unreasonably high i.e a diastolic to systolic peak ratio of > 0.75 (the threshold for unreasonably high must be dependent on how close the peak is to S on the x-axis (sd_time) - lower threshold for closer)   
    if(!is.na(diastPk) & diastPk == avInflexX[peaks[2]]){
      ai. <- avInflexY[peaks[2]] / avInflexY[peaks[1]]
      sd_time <- avInflexX[peaks[2]] - avInflexX[peaks[1]]
      # We move forward through the peaks until finding one that is not unreasonably high (or unreasonably low):
      while( (ai. > 0.45 & sd_time < 75) | ai. > 0.7 | ai. < 0.05 ){   # these tresholds are difficult to get right
        # Assign the previous Dpeak as old, and remove it from the peaks list
        old <- peaks[2]
        peaks <- peaks[-2]
        # Replace the presumed D peak with one that later in time
        # Is the new 2nd highest peak earlier in time? If so we skip it and assign the 3rd value as the dpeak, if not, we assign it as the dpeak. 
        # But if there is no second highest peak now (i.e only systolic remains), se default value:
        if(is.na(peaks[2])){
          diastPk <- 5*sr
          ai. <- 0.5
          sd_time <- 100
        }else{
          if(peaks[2] < old){
            peaks <- peaks[-2]
            if(is.na(peaks[2])){
              diastPk <- 5*sr
              ai. <- 0.5
              sd_time <- 100
            }else{
              diastPk <- avInflexX[peaks[2]] 
              ai. <-  avInflexY[peaks[2]] / avInflexY[peaks[1]]
              sd_time <- avInflexX[peaks[2]] - avInflexX[peaks[1]]
            }
          }else{
            diastPk <- avInflexX[peaks[2]] 
            ai. <- avInflexY[peaks[2]] / avInflexY[peaks[1]]
            sd_time <- avInflexX[peaks[2]] - avInflexX[peaks[1]]
          }
        }
        # If no valid peaks remain, choose the default estimation:
        if(is.na(ai.)){
          diastPk <- 5*sr
          ai. <- 0.5
          sd_time <- 100
        }
      }
    }
  }
  
  # diastPk will be NA for class 3 waveforms, in which case set a default value
  if(is.na(diastPk) | diastPk < avInflexX[peaks[1]]){
    diastPk <- 5*sr
  }
  return(c(diastPk, xShift))
}



osnd_of_average <- function(aw, dp, diff, sr, plot = TRUE){
  
  switch <- 0
  aw <- aw[!is.na(aw)]
  
  avWavPoly <- CubicInterpSplineAsPiecePoly(1:length(aw), aw, "natural")
  sfunction <- splinefun(1:length(aw), aw, method = "natural")
  d1Wav <- sfunction(1:length(aw), deriv = 1)
  d1WavPoly <- CubicInterpSplineAsPiecePoly(1:length(aw), d1Wav, "natural") 
  
  # plot(avWavPoly)
  
  # Find inflexion points on d1WavPoly
  d1InflxX <- solve(d1WavPoly, b = 0, deriv = 1)
  d1InflxY <- predict(d1WavPoly, d1InflxX)
  
  # plot(d1WavPoly)
  # points(d1InflxX, d1InflxY)
  
  # Find OSND
  wavInflxX <- solve(avWavPoly, b = 0, deriv = 1)
  wavInflxY <- predict(avWavPoly, wavInflxX)
  
  # Finding notch based on x-axis:
  # Find inflexion point closest to where the notch usually is (aka 75-80)
  notchRange <- which(d1InflxX > (3.104572*sr - diff) & d1InflxX < dp) # 3.104572 used to be 3.5! #  dp used to be 5*sampling rate!
  # If there is no inflexion point detected within the notch range, this could be because there is a plateu rather than a peak
  # In this case, taking the mean value of the notch range boundaries gives a reasonable approximation
  if(length(notchRange) < 1 | (length(notchRange) == 1 & d1InflxY[notchRange][1] < -0.02)){
    new.n <- ((3.104572*sr) + dp)/2
    # In case new.n is greater than the number of datapoints (or very close to the end), set D to last inflection point on derivative (assuming the wave is very short)
    if(new.n > length(aw) | length(aw) - new.n < (5*(length(aw)/100)) ){
      new.n <- d1InflxX[length(d1InflxX)]
    }
  }else{
    if(length(notchRange) != 1){
      a. <- which(d1InflxY[notchRange] == max(d1InflxY[notchRange]))
      # In cases where the renal peak is higher on 1st deriv than the notch peak, make sure the notch peak is limited by x-axis
      while(d1InflxX[notchRange[a.]] < sr*3){   # was 115 instead of sr*3
        b. <- 2
        a. <- order(d1InflxY[notchRange], decreasing = TRUE)[b.]
        b. <- 3
      }
      # Make sure the 1st peak is not the notch:
      if(which(d1InflxY == max(d1InflxY)) == notchRange[a.]){
        if(length(notchRange[which(notchRange > notchRange[a.])]) > 0){
          notchRange <- notchRange[which(notchRange > notchRange[a.])] 
        }
      }
      if(is.na(d1InflxX[notchRange[a.]])){  # Added this due to bug with participant 7
        a. <- which(d1InflxY[notchRange] == max(d1InflxY[notchRange]))
        new.n <- d1InflxX[notchRange[a.]-1]
      }else{
        new.n <- d1InflxX[notchRange[a.]]
      }
    }else{
      new.n <- d1InflxX[notchRange[1]]
    }
  }
  new.ny <- predict(avWavPoly, new.n)
  # plot(d1WavPoly)
  # points(new.n, predict(d1WavPoly, new.n))
  
  
  # If there are no inflection points before new.n, assume it is incorrect and move to the next inflection point that has a lower y-value than it:
  if(sum(which(wavInflxX < new.n)) < 1){ 
    new.n <- wavInflxX[min(which(wavInflxX > new.n & wavInflxY < new.ny))]
    new.ny <- predict(avWavPoly, new.n)
    } 
 
  
  # After having found the notch on all waves, you can see if there are inflexion points either side:
  # If inflexion point before is lower and inflexion point after is higher (on y axis), this must mean a second peak aka canonical wave
  # Thus if this criterion is fulfilled you can create N and D separately 

  
    if(length(wavInflxX) > 1 & wavInflxY[max(which(wavInflxX < new.n))] < new.ny){
      new.n <- wavInflxX[max(which(wavInflxX < new.n))]
      new.ny <-  predict(avWavPoly, new.n)
      
      # Take the inflection point after the notch as the d peak. If there is no inflection point after the notch, take instead the next inflection point on deriv1
      if(sum(wavInflxX > new.n) > 0){
        d. <- wavInflxX[min(which(wavInflxX > new.n))]
        d.y <- predict(avWavPoly, d.)
      }else{
        d. <- d1InflxX[min(which(d1InflxX > new.n))]
        d.y <- predict(avWavPoly, d.)
      }
      switch <- 1
   }


  
  if(plot == TRUE){
    plot(avWavPoly)
    points(wavInflxX, wavInflxY)
    points(new.n, new.ny, col = "red")
    if(switch == 1){
      points(d., d.y, col = "blue")
    }
  }
  
  
  # Find W: the max inflection point on first deriv:
  w. <- d1InflxX[which( d1InflxY ==  max(d1InflxY) & d1InflxX[which(d1InflxY ==  max(d1InflxY))] < new.n)]
  if(length(w.) < 1){
    w. <- 1
  }
  w.y <- predict(avWavPoly, w.)
  if(plot ==TRUE){points(w., w.y)}
  
  # Find U and V:
  # Find half the height of w (on derivative y-axis)
  hhaw <- max(d1InflxY)/2
  # Find u and v for derivative:
  halfHeights <- solve(d1WavPoly, b = hhaw)
  #halfHeightsY <- predict(d1WavPoly, halfHeights)
  
  # If only one half height detected, assume the segment had quite a high O value such that O was higher than U:
  if(length(halfHeights) < 2){
    halfHeights[2] <- halfHeights[1]
    halfHeights[1] <- solve(d1WavPoly, b = d1Wav[1])[1]
  }
  
  # If more than one half height detected:
  if(length(halfHeights) > 2){
    # Find the distance between each detected half height, compare their xvals to the peak, and keep only the two that have the most similar distance to the peak... 
    # bear in mind, one has to be either side of w...
    a <- halfHeights - w.
    postW <- which(a > 0)
    preW <- which(a < 0)
    # If no points are identified before W (e.g if the wave is too steep to begin with, take the first value as you)
    if(length(preW) < 1){
      print("No valid U found, first value taken as proxy")
      halfHeights <- c(1, min(halfHeights[postW]))
    }else{
      halfHeights <- c(max(halfHeights[preW]), min(halfHeights[postW])) 
    }
  }
  
  u <- halfHeights[1]
  v <- halfHeights[2] 
  # Find u and v y-values for original wave:
  uvY <- predict(avWavPoly, halfHeights)
  uY <- uvY[1]
  vY <- uvY[2]
  if(plot == TRUE){
    points(u, uY)
    points(v, vY) 
  }
  
  # If there are inflection points before w, use the maximum one as 0:
  if(length(which(wavInflxX < w.)) > 0){
    o. <- wavInflxX[max(which(wavInflxX < w.))]
  }else{
    # If there are no inflection points before w, check if there are any in the first deriv before w:
    if(length(which(d1InflxX < w.)) < 1){
      # If not, assign O to the first value
      o. <- 1
    }else{
      # If there are, make sure the max inflection point is not greater than 0 on the original y-axis (which would be higher than U):
      if((predict(avWavPoly, d1InflxX[max(which(d1InflxX < w.))]) > 0)){
        # If the max inflection point is above 0, assign O to the first value
        o. <- 1
      }else{
        # If the max inflection point is not above 0, assign O to it:
        o. <- d1InflxX[max(which(d1InflxX < w.))]
      }
    }
  }
  o._yval <- predict(avWavPoly, o.)
  if(plot == TRUE){points(o., o._yval, pch = 19)}
  
  
  ## Find S:
  # Find new S:
  if( (v - w.) > 100 ){    # if v is erroneous, then this part will not generate a viable alternative to s1
    s2Y <- max(wavInflxY)
    s2 <- wavInflxX[which(wavInflxY == max(wavInflxY))]
  }else{
    s2 <- w. + 2*(abs(v - w.))
    s2Y <- predict(avWavPoly, s2)
  }        
  # points(s2, s2Y)
  # Define old s:
  s1Y <- max(wavInflxY)
  s1 <- wavInflxX[which(wavInflxY == max(wavInflxY))]
  # Decide which S to use...
  if((s1 - w.) < (s2 - w.)){
    s. <- s1
    s.y <- s1Y
  }else{
    s. <- s2
    s.y <- s2Y
  }
  if(plot == TRUE){points(s., s.y, pch = 19)} 
  
  if(switch == 0){
    x <- c(o., s., new.n, new.n)
  }else{
    x <- c(o., s., new.n, d.)
  }
  
  if(switch == 0){
    y <- c(o._yval, s.y, new.ny, new.ny)
  }else{
    y <- c(o._yval, s.y, new.ny, d.y)
  }
  
  osnd <- data.frame(x, y)
  return(osnd)
}


feature_extract <- function(oa, p, pw){
  # S_values:
  s_vals <- c()
  for(i in 1:length(oa)){
    s_vals[i] <- oa[[i]]$y[2]
  }
  
  # N_values:
  n_vals <- c()
  for(i in 1:length(oa)){
    n_vals[i] <- oa[[i]]$y[3]
  }
  
  # D_values:
  d_vals <- c()
  for(i in 1:length(oa)){
    d_vals[i] <- oa[[i]]$y[4]
  }
  
  # NP_ratio:
  np_ratio <- c()
  for(i in 1:length(oa)){
    np_ratio[i] <- oa[[i]]$y[3] / oa[[i]]$y[2]
  }
  
  # PPT:
  ppt <- c()
  for(i in 1:length(oa)){
    ppt[i] <- oa[[i]]$x[4] - oa[[i]]$x[2]
  }
  
  # Maximum amplitude:
  max_amp <- c()
  for(i in 1:length(oa)){
    inx <- solve(pw[[i]], b = 0, deriv = 1)
    iny <- predict(pw[[i]], inx)
    max_amp[i] <- max(iny)
  }
  
  # Total AUC:
  auc <- c()
  for(i in 1:length(oa)){
    wave <- p[, (i+1)]
    v <- which(!is.na(wave))
    auc[i] <- AUC(x = v, y = wave[v], method = "spline")
  }
  
  # AUC after peak of systole:
  auc_s <- c()
  for(i in 1:length(oa)){
    wave <- p[, (i+1)]
    wave <- wave[!is.na(wave)]
    s <- oa[[i]]$x[2]
    v <- 1:length(wave)
    v <- which(v > s)
    auc_s[i] <- AUC(x = v, y = wave[v], method = "spline")
  }
  
  # Length:
  l <- c()
  for(i in 1:length(oa)){
    l[i] <- length(p[, (i+1)][!is.na(p[, (i+1)])])
  }
  
  # Inflexion point area ratio (For canonical waveform use x[3], if using inflection point then use x[4]):
  ipa_ratio <- c()
  for(i in 1:length(oa)){
    wave <- p[, (i+1)][!is.na(p[, (i+1)])]
    ip <- oa[[i]]$x[3]
    v <- 1:length(wave)
    v <- which(v < ip)
    s_auc <- AUC(x = v, y = wave[v], method = "spline")
    v <- 1:length(wave)
    v <- which(v > ip)
    d_auc <- AUC(x = v, y = wave[v], method = "spline")
    ipa_ratio[i] <- s_auc/d_auc
  }
  
  # Peak to Notch time (relative to length of wave):
  pn_time <- c()
  for(i in 1:length(oa)){
    pn_time[i] <- (oa[[i]]$x[3] - oa[[i]]$x[2])/l[i]
  }
  
  # Notch-time ratio = time interval from notch to end of p / time interval from notch to beginning of p:
  nt_ratio <- c()
  for(i in 1:length(oa)){
    wave <- p[, (i+1)][!is.na(p[, (i+1)])]
    # Find next_o i.e the last value of the wave:
    next_o <- max(which(is.na(wave) ==  0)) 
    nt_ratio[i] <- (next_o - oa[[i]]$x[3]) / (oa[[i]]$x[3] -  oa[[i]]$x[1])
  }
  
  # Reflectance peak to forward peak ratio (augmentation index):
  ai <- c()
  for(i in 1:length(oa)){
    ai[i] <- oa[[i]]$y[4] / oa[[i]]$y[2]
  }
  
  # Alternative augmentation index:
  aai <- c()
  for(i in 1:length(oa)){
    aai[i] <- (oa[[i]]$y[2] - oa[[i]]$y[4]) / oa[[i]]$y[2]
  }
  
  # Crest time (O to S time):
  ct <- c()
  for(i in 1:length(oa)){
    ct[i] <- oa[[i]]$x[2] - oa[[i]]$x[1]
  }
  
  features <- data.frame(s_vals, n_vals, d_vals, np_ratio, ppt, max_amp, auc, auc_s, l, ipa_ratio, pn_time, nt_ratio, ai, aai, ct)
  return(features)
}



# References:
# Elgendi et al, 2018: Toward generating more diagnostic features from photoplethysmogram waveforms