# Solve a set of second-order differential equations
#  dy/dt = F(x,y,t) y=ω
#  dx/dt =G(x,y,t)　x=θ

# テンワの慣性モーメント I [Kgm^2]  
I=1.4e-9

# ヒゲゼンマイのバネ定数 k [Nm/rad]
k=4.97428e-7 

# 固体摩擦力 R [Nm]
R=3e-9  
r=R/k

# 粘性減衰係数　c [Nms]
c=1e-10 
h=c/k
# 粘性減衰比率 j
j=c/(2*sqrt(I*k))

# 振動数 f
f=6

# 固有振動数 wn
wn=f*pi

# 衝撃トルク M [Nm]
M=2.18e-7 
m=M/k

# 解除開始テンプ角度 a [degree]
a=-25 #degree

# 解除終了テンプ角度 b [degree]
b=-15 #degree

# 衝撃開始テンプ角度 g [degree]
g=-13 #degree

# 衝撃終了テンプ角度 q [degree]
q=22 #degree

# 解除トルクと衝撃トルクの比 e
e=0.3

# Runge-Kuttaステップの時間幅とステップ数
dt = 0.000001
N  = 340000

# xの初期値 x0 [rad]
x0=-5.5

# y=dx/dtの初期値 y0 [rad/s]
y0=0

# zの初期値（理論初期値） z0 [rad]
z0=-5.5

# 時間シフトの初期値 s0 [s]
s0=0

# 微分方程式の右辺(ここで運動方程式を切り替える）
F(x,y,t)= y > 0 ? (x < a*pi/180 ? -wn**2*(h*y +x+sgn(yold)*r)\
     :(x < b*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r+sgn(yold)*e*m)\
     :(x < g*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r)\
     :(x < q*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r-sgn(yold)*m)\
     :-wn**2*(h*y+x+sgn(yold)*r)))))\
     :(x > -a*pi/180 ? -wn**2*(h*y +x+sgn(yold)*r)\
     :(x > -b*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r+sgn(yold)*e*m)\
     :(x > -g*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r)\
     :(x > -q*pi/180 ? -wn**2*(h*y+x+sgn(yold)*r-sgn(yold)*m)\
     :-wn**2*(h*y+x+sgn(yold)*r)))))
G(x,y,t)=y

# Runge-Kutta法の変数の初期化
k1=0.0; k2=0.0; k3=0.0; k4=0.0
m1=0.0; m2=0.0; m3=0.0; m4=0.0

outputfile="differential-eq-output.dat"

set samples N
set xrange [0:dt*(N-1)]

# いったん別ファイルにデータを書き出す
set table outputfile
plot xold=x0,yold=y0,zold=z0,Aold=z0,sold=s0,\
     "+" using ($1+dt):\
     (k1=dt*F(xold,yold,$1), \
     m1 =dt*G(xold,yold,$1), \
     k2 =dt*F(xold+m1*0.5,yold+k1*0.5, $1+dt*0.5), \
     m2 =dt*G(xold+m1*0.5,yold+k1*0.5, $1+dt*0.5), \
     k3 =dt*F(xold+m2*0.5,yold+k2*0.5, $1+dt*0.5), \
     m3 =dt*G(xold+m2*0.5,yold+k2*0.5, $1+dt*0.5), \
     k4 =dt*F(xold+m3,yold+k3, $1+dt), \
     m4 =dt*G(xold+m3,yold+k3, $1+dt),\
     ynew=yold+(1/6.0)*(k1+2.0*k2+2.0*k3+k4),\
     xnew=xold+(1/6.0)*(m1+2.0*m2+2.0*m3+m4),\
     ynew*yold <= 0 ? Anew=xnew : Anew=Aold,\
     ynew*yold <= 0 ? snew=$1 : snew=sold,\
     znew=(Anew+sgn(ynew)*r)*exp(-j*wn*($1-snew))*cos((sqrt(1-j**2)) \
     *wn*($1-snew)-atan(j/(sqrt(1-j**2))))/(sqrt(1-j**2))-sgn(ynew)*r,\
     xold=xnew, \
     yold=ynew,\
     zold=znew,\
     Aold=Anew,\
     sold=snew,\
     xnew):(ynew):(znew):(Anew):(snew):(0) w xyerrorbars

unset table

# x軸y軸の範囲設定
set xrange [0:0.34]
set yrange [-6:6]

# グラフの縦横比の設定
set size ratio 1

# グラフの格子設定
set grid #

# x軸y軸のラベル付け
set xlabel "t [sec]"
set ylabel "{/=30 {/Symbol-Oblique q}} [rad]"

# グラフの線種設定
set style line 1 linetype 1
set style line 1 linecolor -1
set style line 1 linewidth 2

set style line 2 linetype 1
set style line 2 linecolor 1
set style line 2 linewidth 1

# グラフの描画
plot outputfile u 1:2 w l linestyle 1  title "rk-4",\
     0 w l linecolor -1 notitle
#plot outputfile u 1:4 w l linestyle 2 title "theoretical"

# 出力フォーマットとオプションの指定
set terminal jpeg

# 出力ファイル名の指定
set output "escapement error.jpeg"
# グラフ再描画
replot

# フォーマットと出力のリセット
set output
set terminal win
[EOF]