############################################################
###################### Taylor's method ######################
############################################################

#Inputs: initial time t0; initial value P0; step size h;
# final time tn; vector field f

#matrix field (of own choice)
field <- function(t){
  m12 <- 0.0004 + 10^(0.06*t-5.46)
  m13 <- 0.0005 + 10^(0.038*t -4.12)
  m11 <- -m12-m13
  m21 <- 0.05
  m23 <- m13
  m22 <- -m21-m23
  m31 <- m32 <- m33 <- 0
  L <- matrix(c(m11,m12,m13,m21,m22,m23,m31,m32,m33), nrow=3, byrow=TRUE)
  
  return(L)
}


Taylor <- function(t0,P0,h,tn){
  if(t0==tn){ return(P0)}
  N<- (tn-t0)/h
  D <- dim(field(0))[1]
  #Initial condition at s
  P <- array(diag(D*(N+1)), dim=c(D,D,N+1))
  #First iteration
  P[,,1] <- diag(D) + (h/2)*field(t0)+(h/2)*field(t0+h) + (h^2/2)*field(t0)%*%field(t0)
  
  
  for(n in 1:N){
    aux <- field(t0+n*h)
    aux2 <- field(t0+n*h+h)
    P[,,n+1] <- P[,,n]%*%(diag(D) + (h/2)*aux+ (h/2)*aux2 + (h^2/2)*aux%*%aux)
  }
  return(P)
}

t0 <- 30
P0 <- diag(3)
tn <- 110
h <- 1/12
N <- (tn-t0)/h

Taylor_sol <- Taylor(t0,P0,h,tn)

plot(seq(t0,tn,by=h), Taylor_sol[1,1,], type="l", col="blue", main="Transition probabilities", xlab="Age", ylab="Probability")
lines(seq(t0,tn,by=h), Taylor_sol[1,2,], type="l", col="yellow")
lines(seq(t0,tn,by=h), Taylor_sol[1,3,], type="l", col="red")
lines(seq(t0,tn,by=h), Taylor_sol[2,1,], type="l", col="green") #reactivation
legend("right", legend=c("survival", "disability", "death", "reactivation"), col=c("blue", "yellow", "red", "green"), lty=1, cex=0.8)

#Comparison Euler vs. Taylor
plot(seq(t0,tn,by=h), Euler_sol[1,1,], type="l", col="red", main="Transition probabilities", xlab="Age", ylab="Probability")
lines(seq(t0,tn,by=h), Taylor_sol[1,1,], type="l", col="blue")
legend("right", legend=c("Euler", "Taylor"), col=c("red", "blue"), lty=1, cex=0.8)
plot(seq(t0,tn,by=h), Euler_sol[1,1,]-Taylor_sol[1,1,], type="l", col="red", main="Transition probabilities", xlab="Age", ylab="Probability")
legend("topright", legend=c("Difference Euler-Taylor"), col=c("red"), lty=1, cex=0.8)