library(ggplot2)
library(signal)

source("defines.R")

# Cyclus Measurements
cyclus <- read.csv("cyclusintervals.csv")
# Mechanisches Gangverhältnis
gear_ratio <- cyclus$ErgoFrontGear[1]/cyclus$ErgoBackGear[1]

# SRM Measurements
# 0: Cadence
# 1: Torque
srm <- read.csv("srmintervals.csv")
srm_cadence <- plyr::rename(subset(srm, Type==0, select=c("Time", "Value")), c("Value"="Cadence"))
srm_torque <- plyr::rename(subset(srm, Type==1, select=c("Time", "Value")), c("Value"="Torque"))

# Arduino Measurements
# 0: Photo Sensor (back, 25 teeth, Value: time difference)
# 1: Reed Switch (front, Value: time difference)
# 2: Reed Switch (back, Value: time difference)
arduino <- read.csv("arduinointervals.csv")
arduino_photoBack <- plyr::rename(subset(arduino, Type==0, select=c("Time", "Value")), c("Value"="TimeDiff"))
arduino_photoBack$RPS <- 1/25 / arduino_photoBack$TimeDiff
arduino_photoBack$Cadence <- arduino_photoBack$RPS / gear_ratio * 60
arduino_reedFront <- plyr::rename(subset(arduino, Type==1, select=c("Time", "Value")), c("Value"="TimeDiff"))
arduino_reedFront$Cadence <- 1 / arduino_reedFront$TimeDiff * 60
arduino_reedBack <- plyr::rename(subset(arduino, Type==2, select=c("Time", "Value")), c("Value"="TimeDiff"))
arduino_reedBack$Cadence <- 1 / arduino_reedBack$TimeDiff / gear_ratio * 60

# Frequency response of Cadence collected with the optical sensor
rps_intervall_arduino <- subset(arduino_photoBack, Time > 190 & Time < 210, select = c("Time", "TimeDiff", "Cadence")) # select intervall for cadence detection
fs = 100 # sampling frequency in Hertz (Hz): at least twice of the dominant signal frequency (https://de.wikipedia.org/wiki/Nyquist-Shannon-Abtasttheorem)
Ts = 1/fs # sampling rate
t <- seq(rps_intervall_arduino$Time[1], last(rps_intervall_arduino$Time), by=Ts) # sampling vector (time domain)
x <- approx(rps_intervall_arduino$Time, rps_intervall_arduino$TimeDiff, t)[[2]] # interpolation at points from sampling vector
x <- x - mean(x) # substract mean -> avoid large DC component
X <- fft(x) # FFT
freqs <- seq(-fs/2, fs/2, length.out = length(x)) # Frequency domain (-fs/2 to fs/2)
X1 <- abs(waved::fftshift(X))/length(x) # shift DC to center
plot(freqs,X1,type='l') 
X2 <- spectral::spec.fft(y=x, x=t, center = TRUE) # second option: fft result already centered
lines(X2[[1]], abs(X2[[2]]), col="red")
X <- X/length(X) # normalize fft result
X <- X[1:(length(X)/2)] # take just the first half
freqs <- seq(0, fs/2, length.out = length(x)/2) # frequencies from 0 to fs/2 (first half)
ggplot() + geom_line(aes(x=freqs, y=abs(X)), size = 2) + plot_style + 
  coord_cartesian(xlim = c(0, 5)) +
  labs(title = "Spectrum", x = "Frequency (Hz)", y = "Amplitude")
peak <- freqs[which(abs(X)==max(abs(X)))] # find peak
singleleg_peak <- peak/2 # take every second for one leg / one round
cad <- singleleg_peak*60 # average cadence by fourrier analysis
print(paste0("Dominant frequency is at ", round(peak,2),
             ". Thus, single leg frequency is ", round(singleleg_peak,2),
             " which equates to a cadence of ", round(cad,2),
             " rpm. Cadence computed from optical sensor data is ",
             round(mean(rps_intervall_arduino$Cadence),2)))

# Frequency response of Cadence collected with SRM Torquebox
rps_intervall_srm <- subset(srm_torque, Time > 190 & Time < 210) # select intervall for cadence detection
fs = 100 # sampling frequency in Hertz (Hz): at least twice of the dominant signal frequency (https://de.wikipedia.org/wiki/Nyquist-Shannon-Abtasttheorem)
rps_intervall_srm_cad <- subset(srm_cadence, Time > 190 & Time < 210) # select intervall for cadence detection
Ts = 1/fs # sampling rate
t <- seq(rps_intervall_srm$Time[1], last(rps_intervall_srm$Time), by=Ts) # sampling vector (time domain)
x <- approx(rps_intervall_srm$Time, rps_intervall_srm$Torque, t)[[2]] # interpolation at points from sampling vector
# x <- as.numeric(filter(rep(1/100,100), 1, x)) # compare with cyclus2
x <- x - mean(x) # substract mean -> avoid large DC component
X <- fft(x) # FFT
freqs <- seq(-fs/2, fs/2, length.out = length(x)) # Frequency domain (-fs/2 to fs/2)
X1 <- abs(waved::fftshift(X))/length(x) # shift DC to center
plot(freqs,X1,type='l') 
X2 <- spectral::spec.fft(y=x, x=t, center = TRUE) # second option: fft result already centered
lines(X2[[1]], abs(X2[[2]]), col="red")
X <- X/length(X) # normalize fft result
X <- X[1:(length(X)/2)] # take just the first half
freqs <- seq(0, fs/2, length.out = length(x)/2) # frequencies from 0 to fs/2 (first half)
ggplot() + geom_line(aes(x=freqs, y=abs(X)), size = 2) + plot_style + 
  coord_cartesian(xlim = c(0, 5)) +
  labs(title = "Spectrum", x = "Frequency (Hz)", y = "Amplitude")
peak <- freqs[which(abs(X)==max(abs(X)))] # find peak
singleleg_peak <- peak/2 # take every second for one leg / one round
cad <- singleleg_peak*60 # average cadence by fourrier analysis
print(paste0("Dominant frequency is at ", round(peak,2),
             ". Thus, single leg frequency is ", round(singleleg_peak,2),
             " which equates to a cadence of ", round(cad,2),
             " rpm. Cadence computed from optical sensor data is ",
             round(mean(rps_intervall_srm_cad$Cadence),2)))

# Frequency response of Cadence collected with Cyclus2
rps_intervall_cyclus <- subset(cyclus, Time > 190 & Time < 210, select = c("Time", "DwDisc", "Cadence")) # select intervall for cadence detection
fs = 100 # sampling frequency in Hertz (Hz): at least twice of the dominant signal frequency (https://de.wikipedia.org/wiki/Nyquist-Shannon-Abtasttheorem)
Ts = 1/fs # sampling rate
t <- seq(rps_intervall_cyclus$Time[1], last(rps_intervall_cyclus$Time), by=Ts) # sampling vector (time domain)
x <- approx(rps_intervall_cyclus$Time, rps_intervall_cyclus$DwDisc, t)[[2]] # interpolation at points from sampling vector
x <- x - mean(x) # substract mean -> avoid large DC component
X <- fft(x) # FFT
freqs <- seq(-fs/2, fs/2, length.out = length(x)) # Frequency domain (-fs/2 to fs/2)
X1 <- abs(waved::fftshift(X))/length(x) # shift DC to center
plot(freqs,X1,type='l') 
X2 <- spectral::spec.fft(y=x, x=t, center = TRUE) # second option: fft result already centered
lines(X2[[1]], abs(X2[[2]]), col="red")
X <- X/length(X) # normalize fft result
X <- X[1:(length(X)/2)] # take just the first half
freqs <- seq(0, fs/2, length.out = length(x)/2) # frequencies from 0 to fs/2 (first half)
ggplot() + geom_line(aes(x=freqs, y=abs(X)), size = 2) + plot_style + 
  coord_cartesian(xlim = c(0, 5)) +
  labs(title = "Spectrum", x = "Frequency (Hz)", y = "Amplitude")
peak <- freqs[which(abs(X)==max(abs(X)))] # find peak
singleleg_peak <- peak/2 # take every second for one leg / one round
cad <- singleleg_peak*60 # average cadence by fourrier analysis
print(paste0("Dominant frequency is at ", round(peak,2),
             ". Thus, single leg frequency is ", round(singleleg_peak,2),
             " which equates to a cadence of ", round(cad,2),
             " rpm. Cadence computed from optical sensor data is ",
             round(mean(rps_intervall_cyclus$Cadence),2)))

# Find delay between cyclus and photo sensor (Cross correlation)
# between 10 and 90 seconds
startTime <- 10
endTime <- 90
sr <- 0.01
time <-seq(startTime, endTime, by=sr) # time vector
photo_cadence <- approx(arduino_photoBack$Time, arduino_photoBack$Cadence, time) # approximate/interpolate values to time vector
photo_cadence <- photo_cadence[[2]] - mean(photo_cadence[[2]]) # substract mean (small cross-correlation result)
cyclus_cadence <- approx(cyclus$Time, cyclus$Cadence, time) # approximate/interpolate values to time vector
cyclus_cadence <- cyclus_cadence[[2]] - mean(cyclus_cadence[[2]]) # substract mean (small cross-correlation result)

# Cross Correlation: (f*g)[n]=\sum_{m=-\infty}^{infty}f*[m]*g[m+n] (take 400 for infty)
# https://en.wikipedia.org/wiki/Cross-correlation (second formula)
N <- 400
crosscorr <- numeric(2*N+1) 
for (n in -N:N)
{
  crosscorr[N+1+n] = sum(photo_cadence[(N+1):(length(photo_cadence)-N)]*cyclus_cadence[(N+1+n):(length(cyclus_cadence)-N+n)])
}
crosscorr <- crosscorr/max(crosscorr)
delay <- (-N:N)[which(crosscorr == max(crosscorr))[[1]]]*(time[2]-time[1]) # find index of delay and multiply by t[i+1]-t[i] (sampling rate)
t <- (-N:N)*(time[2]-time[1])

ggplot() + geom_line(aes(x=t, y=crosscorr), size = 2) + plot_style + 
  geom_vline(xintercept = delay, color="red") +
  labs(title = "Cross-Correlation", x = "Time (s)", y = "") +
  scale_x_continuous(breaks = c(-2,-1,0,round(delay,2),1,2), labels = c("-2","-1",0,round(delay,2),"1","2"))

# Use existing R function
res <- ccf(cyclus_cadence, photo_cadence, pl = FALSE, lag.max=N)
res$acf <- res$acf/max(res$acf)
res$lag <- res$lag * sr
plot(res)

print(delay)
print(res$lag[res$acf == max(res$acf)])