!***********************************************************************
!	Dr.田崎エンジンのシミュレーション
!	Date: 2008/01/07
!	g95 DrTasaki.F90 -o3 -Wno=165 -Wall -O -o DrTasaki -Wl,--subsystem,console -lGrWin -mwindows
PROGRAM DrTasaki
IMPLICIT NONE

! GWライブラリについては以下の警告が出る
! Warning (165): Implicit interface 'gwopen' called at (1)
! 気になるのであれば、-Wno=165 として警告を抑制する

	! 物体の数、1=固定の大きなベース、2以降=ベースに付いた振動子
	INTEGER, PARAMETER :: N_BALL = 7
	! バネの自然長
	INTEGER, PARAMETER :: LENGTH = 100
	! 初期平均速度、全体がこの速度で走っている
	REAL, PARAMETER :: BASE_V = 100.0
	! 加わるエネルギー
	REAL, PARAMETER :: ACC = 100.0
	
	! 位相がばらばらな場合の初期位置、適当な変位を入れてみよう！
	REAL :: x(1:N_BALL)  = (/0.0, 95.0, 120.0, 110.0, 105.0, 90.0, 88.0/)
	! 位相がそろっている場合の初期位置
!!	REAL :: x(1:N_BALL)  = (/0.0, 120.0, 120.0, 120.0, 120.0, 120.0, 120.0/)
	
	REAL :: next_x(1:N_BALL)	! 1ステップ先の位置
	REAL :: dx(1:N_BALL) = (/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/)
	REAL :: next_dx(1:N_BALL)
	
	REAL :: t=0.0
!	REAL :: dt=0.1	! 計差精度粗くしてみる
	REAL :: dt=0.05 ! 計算精度標準
!	REAL :: dt=0.008	!高精度
	
	! 描画する物体の高さ、大きさ
	REAL, PARAMETER :: yy=250.0, rr=8.0
	
	INTEGER :: i, acc_count = 0
#ifndef NOGR
	INTEGER :: IR, NOG=1	!GWライブラリ関連
#endif

! BEGIN
#ifndef NOGR
	CALL GWopen(IR,0)
	CALL GWindow(IR, -200.0, 0.0, 300.0, 300.0)
	CALL GWsetogn(IR, 0, -NOG)
#endif
	DO
		! 連成振動の運動を求める
		CALL Runge(t, x, dx, next_x, next_dx, dt, N_BALL)
		
		x = next_x	! 次の位置
			! 単にコピーしているだけなので、ほんとはメモリーの無駄ですね...
		
		! 1000回ごとに加速する
		IF (acc_count > 1000) THEN
			print *, 'Accel!'
			
			! 全体がこの速度で走っているものとする
			DO i=1, N_BALL
				next_dx(i) = next_dx(i) + BASE_V
			END DO
			
			! 加速する
			DO i=1, N_BALL
				dx(i) = Accel( next_dx(i) )
			END DO
			
			! その直後に減速する
			CALL Brake( dx, N_BALL )
			
			! 全体の走っている速度を引く
			DO i=1, N_BALL
				dx(i) = dx(i) - BASE_V
			END DO
			
			acc_count = 0	! 最初に戻る
		ELSE	! 普段はそのまま
			dx = next_dx
			acc_count = acc_count + 1
		END IF
		
#ifndef NOGR
		! 描画、１個目を描く
		CALL GWsrect(IR, x(1)-rr, yy, x(1)+rr, yy - 20.0*N_BALL - 30.0, 5 ) ! 5=紫
		
		! ２個目以降を描く
		CALL GWsetpen(IR, 2, -1, -1, -1)	! 2=暗緑色
		CALL GWsetbrs(IR, 2, 1, -1)	!塗りつぶし
		DO i=2, N_BALL
			CALL GWellipse(IR, x(i)-rr, yy-rr - i*20.0, x(i)+rr, yy+rr - i*20.0)
		END DO
		
		CALL GWflush(IR, -NOG)
#endif
	END DO
	
#ifndef NOGR
	CALL GWSETBK(IR, 1)
	CALL GWREFRESH(IR)
	CALL GWQUIT(IR)
#endif

CONTAINS

!**********************************************************************
! ポテンシャルの坂を下って加速する
! 返り値: 加速後の速度
REAL FUNCTION Accel(v)
IMPLICIT NONE
	REAL, INTENT(IN) :: v
	real :: e
	
	e = v ** 2 / 2.0	! エネルギーを求める
	
	IF (v >= 0) THEN
		Accel = sqrt( (e + ACC) * 2.0 )
	ELSE	! 方向が逆の場合、まず既存のエネルギーから引く
		e = ACC - e
		IF (e > 0) THEN	! その結果が正であれば
			Accel = - sqrt( e * 2.0 )	! まだ速度は負のまま
		ELSE	! 結果が負であれば
			Accel = sqrt( -e * 2.0 )	! 結果は正になる
		END IF
	END IF
	
	RETURN
END FUNCTION Accel

!**********************************************************************
! ポテンシャルの坂を上って減速する
SUBROUTINE Brake(v, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: N
	REAL, INTENT(INOUT) :: v(N)
	REAL :: ave_p, dec_e, dec_v, diff_v
	INTEGER :: i
	
	! 全体の平均運動量を求める
	ave_p = v(1) * (N-1)	! ベースの質量は N-1
	DO i=2, N
		ave_p = ave_p + v(i)
	END DO
	ave_p = ave_p / (2 * (N-1))	! ベースの質量がN-1、それ以外のボールがN-1個
	
	! 減速する速度を求める
	IF (ave_p > 0) THEN
		dec_e =   (ave_p ** 2 / 2.0) - ACC
	ELSE
		dec_e = - (ave_p ** 2 / 2.0) - ACC
	END IF
	
	IF (dec_e > 0) THEN
		dec_v = sqrt(   dec_e * 2.0)
	ELSE
		dec_v = sqrt( - dec_e * 2.0)
	END IF
	
	diff_v = ave_p - dec_v	! 速度の差はこれだけになる
	
	! 全体を一斉に減速する
	DO i=1, N
		v(i) = v(i) - diff_v
	END DO
	
	RETURN
END SUBROUTINE Brake

!**********************************************************************
! xの導関数
REAL FUNCTION fx(t, x, v, i, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, N
	REAL, INTENT(IN) :: t, x(N), v(N)
	fx = v(i)	! 定数=1.0
	return
END FUNCTION fx

!**********************************************************************
! vの導関数
REAL FUNCTION fv(t, x, v, i, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, N
	REAL, INTENT(IN) ::  t, x(i), v(i)
	REAL :: sfv
	INTEGER :: j
	
	! １個目(ベース)は残りの全てと引き合う
	IF (i == 1) THEN
		sfv = 0.0
		DO j=2,N
			sfv = sfv + (x(j) - LENGTH - x(1))
		END DO
		fv = sfv / (N-1)	! ベースの質量=(N-1)とする
	! それ以外は１個目(ベース)と引き合う
	ELSE
		fv = x(1) + LENGTH - x(i)
	END IF
	
	return
END FUNCTION fv

!**********************************************************************
! ルンゲ・クッタ法
SUBROUTINE Runge(t, x, v, xout, vout, dt, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: N
	REAL, INTENT(INOUT) :: t
	REAL, INTENT(IN) :: x(:), v(:)
	REAL, INTENT(OUT) :: xout(:), vout(:)
	REAL, INTENT(IN) :: dt
	real :: h1
	real :: kx(4,N), kv(4,N)
	real :: kx1(N), x1(N), x2(N), x3(N)
	real :: kv1(N), v1(N), v2(N), v3(N)
	integer :: i

	h1 = dt/2.0
	DO i=1,N
		kx(1,i) = fx(t, x, v, i, N) * dt
		kv(1,i) = fv(t, x, v, i, N) * dt
		x1(i) = x(i) + kx(1,i)/2.0
		v1(i) = v(i) + kv(1,i)/2.0
	END DO
	DO i=1,N
		kx(2,i) = fx(t+h1, x1, v1, i, N) * dt
		kv(2,i) = fv(t+h1, x1, v1, i, N) * dt
		x2(i) = x(i) + kx(2,i)/2.0
		v2(i) = v(i) + kv(2,i)/2.0
	END DO
	DO i=1,N
		kx(3,i) = fx(t+h1, x2, v2, i, N) * dt
		kv(3,i) = fv(t+h1, x2, v2, i, N) * dt
		x3(i) = x(i) + kx(3,i)
		v3(i) = v(i) + kv(3,i)
	END DO
	DO i=1,N
		kx(4,i) = fx(t+dt, x3, v3, i, N) * dt
		kv(4,i) = fv(t+dt, x3, v3, i, N) * dt
	END DO

	t = t+dt
	DO i=1,N
		kx1(i) = (kx(1,i) + 2.0*kx(2,i) + 2.0*kx(3,i) + kx(4,i)) / 6.0
		kv1(i) = (kv(1,i) + 2.0*kv(2,i) + 2.0*kv(3,i) + kv(4,i)) / 6.0
	END DO

!	DO i=1,N
!		xout(i) = x(i)+kx1(i)
!		vout(i) = v(i)+kv1(i)
!	END DO
		xout = x + kx1
		vout = v + kv1

	RETURN
END SUBROUTINE Runge

END PROGRAM DrTasaki
!*** END of FILE *******************************************************