# -*- coding: utf-8 -*-
"""
Created on Thu Mar 23 02:05:44 2017
RK4 Method
University of Sao Paulo
@author: Gabriel
"""

def rk4(ini,end,step,x0,u0,P,deltap,u,j):
    
    from numpy import *
    import copy
    
    t = ini
    x = x0.T
#    print x
#    x = array([[P[6]],[P[7]],[x[2,0]]])
#    print x
    p = copy.copy(P)
    
    if j != -1:
        p[j] += deltap[j]

    A,B,C,D = Matrix(p, x0,u0)
    
    y = dot(C,x) + dot(D,u)
    
    output = append(t, y.T)
	
    while t < end:
        K1 = step*(dot(A,x) + dot(B,u))
        K2 = step*(dot(A,x + K1) + dot(B,u)) #x.T é a transposta de x
        K3 = step*(dot(A,x + K2) + dot(B,u))
        K4 = step*(dot(A,x + K3) + dot(B,u))
        
        t += step

        x = x + (K1 + 2*K2 + 2*K3 + K4)/6
        y = dot(C,x) + dot(D,u)
#        if t == ini + step:
#            print "\n\nC*x\n"
#            print C, x
#            print dot(C,x)
#            print "\n\nD*u\n"
#            print D, u
#            print dot(D,u)
#            print "\n\ny\n"
#            print y
        
        output = vstack((output, append(t, y.T)))

    return output