!***********************************************************************
!	振動子と粒子の衝突
!	ランダムにエネルギーを分配した熱浴との接触
!	Date: 2007/07/16
!	g95 MaxDemon12.F90 thpool_N2.o mtfort90.o -o3 -Wno=165 -Wall -O -o MaxDemon12 -Wl,--subsystem,console -lGrWin -mwindows
PROGRAM MaxDemon12
USE thpool_N2	! 熱浴モジュール使用、内部でMT乱数モジュール使用
IMPLICIT NONE

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

	! 引数
	CHARACTER*8 :: str
	INTEGER :: param1
	! 物体の数、1=等温壁、2=ボール、3=隔壁、4=ボール、5=等温壁
	INTEGER, PARAMETER :: N_BALL = 5
	! 振動子の数（左右で独立、連動してはいない）
	INTEGER, PARAMETER :: N_BODY = 2
	! 画面サイズ
	INTEGER, PARAMETER :: WIDTH = 500, HEIGHT = 300
	! 壁の位置
	REAL*8, PARAMETER :: W1POS = 0.0d0
	! 描画する物体の高さ、大きさ
	REAL*8, PARAMETER :: yy=200.0d0, rr=8.0d0
	
	REAL*8, PARAMETER :: rx0 = 10.0	! 初期振動子の振幅
	REAL*8 :: x(1:N_BALL)  = (/rx0, 125.0d0, 250.0d0, 375.0d0, real(WIDTH)-rx0/)
	REAL*8 :: next_x(1:N_BALL)	! 1ステップ先の位置
	REAL*8 :: dx(1:N_BALL) = (/0.0d0, 10.0d0, 0.0d0, 10.0d0, 0.0d0/)
	REAL*8 :: m(1:N_BALL)  = (/1.0d0, 1.0d0, 5.0d0, 1.0d0, 1.0d0/)
	REAL*8 :: t=0.0d0
!	REAL*8 :: dt=0.1d0	! 計差精度粗くしてみる
	REAL*8 :: dt=0.05d0 ! 計算精度標準
!	REAL*8 :: dt=0.008d0	!高精度
	
	! 連成振動子の位置と速度
	REAL*8 :: rx(1:N_BODY) = (/rx0, real(WIDTH)-rx0/)
	REAL*8 :: rdx(1:N_BODY) = (/0.0d0, 0.0d0/)
	REAL*8 :: next_rx(1:N_BODY)
	REAL*8 :: next_rdx(1:N_BODY)
	REAL*8, PARAMETER :: yy2 = 100.0d0
	REAL*8 :: ave_eg1				!振動子の平均エネルギー
	REAL*8 :: init_eg, total_eg, diff_eg	!全エネルギーを一定に保つ
	INTEGER :: diff_idx = 1
	
	! ワープゾーンの位置
	INTEGER, PARAMETER :: DELTA_P = 50
	INTEGER :: P1FROM =  50, P1TO
	INTEGER, PARAMETER :: P2FROM = 225, P2TO = P2FROM+DELTA_P
	
	! ワープ用作業変数
	INTEGER, PARAMETER :: WARP_INTERVAL = 1500
		! 150 -- それでも気になるのでピコピコが生じないほど長くしてみた
		! 20 -- インターバル時間を変えても偏り傾向には影響ない
	INTEGER :: interval=0, warpcnt=0, totwarpcnt=0
	
	! 当たり判定フラグ
	LOGICAL :: colflag(1:N_BALL-1)
	LOGICAL :: acolflag		!１回でも衝突があればtrue
	
	! ヒストグラム
!!	INTEGER, PARAMETER :: N_HIST = 20
!!	INTEGER :: hist(1:N_HIST) = (N_HIST * 0)
!!	INTEGER :: histcnt = 0, pos
#ifdef LIMIT
!!	INTEGER :: tothistcnt = 0
#endif
	INTEGER :: egcnt = 0, egcnt2 = 0	! 定期的にエネルギー表示
	
	INTEGER :: i, iter
#ifndef NOGR
	INTEGER :: IR, NOG=1	!GWライブラリ関連
#endif
	INTEGER, PARAMETER :: MAXITER = 100

! BEGIN
	CALL getarg(1,str) ! 第 1 引数を得る
	param1 = strtoi( str )
	P1FROM = P1FROM + param1
	P1TO = P1FROM + DELTA_P
	PRINT "('P1FROM=',I3,' P1TO=',I3)", P1FROM, P1TO
	
#ifndef NOGR
	CALL GWopen(IR,0)
	CALL GWindow(IR, -20.0, 0.0, real(WIDTH), real(HEIGHT))
	CALL GWsetogn(IR, 0, -NOG)
#endif
	! 振動子エネルギーの初期値を得る
	ave_eg1 = VivEG(       rx(1),      rdx(1),      dt)
!!	ave_eg2 = VivEG( WIDTH-rx(N_BODY), rdx(N_BODY), dt)
	init_eg = pool_init( ave_eg1 )	! 初期エネルギーを得る
	init_eg = init_eg + printEG(rx, rdx, dx, m, dt)
	
	DO
#ifndef NOGR
		! ワープゾーンを描く
		CALL GWsrect(IR, real(P1FROM), 0.0, real(P1TO),real(HEIGHT), 12)
		CALL GWsrect(IR, real(P2FROM), 0.0, real(P2TO),real(HEIGHT), 12)
#endif
		! 衝突ループ
		DO iter=1, MAXITER
		
			! 連成振動の運動を求める
			CALL Runge(t, rx, rdx, next_rx, next_rdx, dt, N_BODY)
			! 両端のrdxはそのまま等温壁の運動となる
			next_x(1)       = next_rx(1)
			next_x(N_BALL)  = next_rx(N_BODY)
			
			! １ステップ先のボールの位置を求める
			DO i=2, N_BALL-1
				next_x(i) = x(i) + dx(i) * dt
			END DO
			
			! 当たり判定
			acolflag = .FALSE.
			DO i=1, N_BALL-1
				colflag(i) = isCol( x, next_x, i, i+1 )
				if ( colflag(i) ) then
!					PRINT "('Hit! (',I2,')',F6.2, F6.2, F6.2, F6.2)", &
!&						iter, x(i), dx(i), x(i+1), dx(i+1)
					acolflag = .TRUE.
				end if
			END DO
			if ( .NOT. acolflag ) then	! 衝突していなければ
				exit	!ループ終了
			end if
			
			! ボールの衝突処理
			DO i=1, N_BALL-1
				if ( colflag(i) ) then
					CALL Collision( dx, m, i, i+1 )
					! 等温壁との衝突処理、振動子との同期をとる
					if (i .eq. 1) then
						rdx(1) = dx(1)
					else if (i .eq. N_BALL-1) then
						rdx(N_BODY) = dx(N_BALL)
					end if
				end if
			END DO
			
		END DO
		if (iter >= MAXITER ) then
			PRINT *,'ERROR: 衝突無限ループ'
			stop
		end if
		
		! 物体の移動
		DO i=1, N_BALL
			x(i) = next_x(i)
		END DO
		
	!	! 順序が保たれているかどうかチェック
	!	IF ( x(2) > x(3) ) THEN
	!		PRINT *, 'ERROR: 順序違反2:3'
	!		x(2) = x(2) - dx(2)
	!	!!	x(3) = x(3) - dx(3)
	!	END IF
	!	IF ( x(3) > x(4) ) THEN
	!		PRINT *, 'ERROR: 順序違反3:4'
	!	!!	x(3) = x(3) - dx(3)
	!		x(4) = x(4) - dx(4)
	!	END IF
		
		! 振動子の位置と速度を確定する
		rx = next_rx
		rdx = next_rdx
		! 両端の等温壁の速度を確定する
		dx(1) = next_rdx(1)
		dx(N_BALL) = next_rdx(N_BODY)
		
		! エネルギーを平均化する
		IF (egcnt .EQ. 0) THEN	! 一定間隔で行う
			CALL AveEG_pool(rx, rdx, 1, dt)
			CALL AveEG_pool(rx, rdx, 2, dt)
		END IF
		CALL pool_run( 1 )
		
		!ワープ処理 -- x(3)を２つのセグメント間で移動する
		IF ( interval == 0 ) THEN
			! 順方向ワープ
			IF( P2FROM < x(3) .and. x(3) < P2TO &
&				.AND. (x(2) < P1FROM) &
&				.AND. (P2TO < x(4)) ) THEN
				x(3) = x(3) - (P2FROM - P1FROM)
				warpcnt = warpcnt + 1
				totwarpcnt = totwarpcnt + 1
				PRINT "('+++ WARP +++', I4, I6)", warpcnt, totwarpcnt
			! 逆方向ワープ -- ここにelseを入れないと対称にならないぞ
			ELSE IF( P1FROM < x(3) .and. x(3) < P1TO &
&				.AND. (x(2) < P1FROM) &
&				.AND. (P2TO < x(4)) ) THEN
				x(3) = x(3) + (P2FROM - P1FROM)
				warpcnt = warpcnt - 1
				totwarpcnt = totwarpcnt + 1
				PRINT "('--- WARP ---', I4, I6)", warpcnt, totwarpcnt
			END IF
			! ワープ後しばらくの間は次のワープができない
			interval = WARP_INTERVAL
		ELSE
			interval = interval - 1
		END IF
		
#ifndef NOGR
		! 描画
		DO i=1, N_BALL, 2
			CALL GWsrect(IR, real(x(i)-rr), real(yy-30.0d0), real(x(i)+rr), real(yy+30.0d0), 5 ) ! 5=紫
		END DO
		! ボールを描く
		CALL GWsetpen(IR, 2, -1, -1, -1)	! 2=暗緑色
		CALL GWsetbrs(IR, 2, 1, -1)	!塗りつぶし
		DO i=2, N_BALL, 2
			CALL GWellipse(IR, real(x(i)-rr), real(yy-rr), real(x(i)+rr), real(yy+rr) )
		END DO
		! 連成振動子を描く
		CALL GWsetpen(IR, 3, -1, -1, -1)	! 3=オリーブ色
		CALL GWsetbrs(IR, 3, 1, -1)	!塗りつぶし
		DO i=1, N_BODY
			CALL GWellipse(IR, real(rx(i)-rr), real(yy2-rr), real(rx(i)+rr), real(yy2+rr) )
		END DO
		! 外枠を描く
		CALL GWsetpen(IR, 4, -1, -1, -1)	! 4=濃紺
		CALL GWsetbrs(IR, -1, 0, -1)	! 中空
		CALL GWrect(IR, 0.0, 50.0, real(WIDTH), real(HEIGHT)-50.0 )
		
		CALL GWflush(IR, -NOG)
#endif
	! 放っておくと誤差が累積するので、エネルギーの補正を行う
	egcnt = egcnt + 1
	IF (egcnt > 100) THEN	! 100回に１回、単に計算を軽くするため
		egcnt = 0
		total_eg = printEG(rx, rdx, dx, m, dt)	! エネルギーの表示
		total_eg = total_eg + pool_total()	! 現在の全エネルギーを求める
		
		egcnt2 = egcnt2 + 1
		IF (egcnt2 > 500) THEN
			egcnt2 = 0
			CALL pool_list	! 熱浴の状態表示
		END IF
		
		diff_eg = total_eg - init_eg
	!!	PRINT *, 'Diff_EG:', diff_eg
		! poolのインデックス、分子は1,2の２つしかないので交互に配分した
		IF (diff_idx == 1) THEN
			diff_idx = 2
		ELSE
			diff_idx = 1
		END IF
		IF (pool_get(diff_idx) - diff_eg > 0.0) THEN	! 負にならないように
			CALL pool_set(diff_idx, pool_get(diff_idx) - diff_eg)
		END IF
	END IF
	
		! ヒストグラムをとる
	!!	histcnt = histcnt + 1
	!!	IF ( histcnt >= 5000 ) THEN ! 5000回に１回サンプリング
	!!		histcnt = 0
	!!		! x(3)の位置を把握する
	!!		pos = int( x(3) / (WIDTH / N_HIST) ) + 1
	!!		hist(pos) = hist(pos) + 1
	!!		PRINT *, hist(1:N_HIST)
#ifdef LIMIT
	!!		tothistcnt = tothistcnt + 1
#endif
	!!	END IF
		
#ifdef LIMIT
		! 3000回でシミュレーションを打ち切る
	!	IF (tothistcnt > 3000) THEN
		IF (totwarpcnt > 3000) THEN
			EXIT
		END IF
#endif
	END DO
	
#ifndef NOGR
	CALL GWSETBK(IR, 1)
	CALL GWREFRESH(IR)
	CALL GWQUIT(IR)
#endif

CONTAINS

!***********************************************************************
! エネルギーの表示
! 両端は -0.0, WIDTH で固定している
! 系の持つ全エネルギーを返す
REAL*8 FUNCTION printEG(rx, rdx, dx, m, dt)
IMPLICIT NONE
	REAL*8, INTENT(IN) :: rx(:), rdx(:), dx(:), m(:), dt
	REAL*8 :: eg0v, eg1v, eg0p, eg1p, eg2, eg3, eg4

	! 振動子の運動エネルギー
!	eg1v = 0.0
!	DO i=1,N_BODY
!		eg0v = dt * rdx(i) ** 2 / 2.0		!質量は1.0
!!		PRINT "('v',I2,': ',F11.3,': ',F11.3)", i, rdx(i), eg0v
!		eg1v = eg1v + eg0v
!	END DO
	
!	! 振動子の位置エネルギー（左右独立）
!	eg1p = 0.0
!	DO i=1,N_BODY
!		eg0p = dt * (WIDTH*(i-1) - rx(i)) ** 2 / 2.0 !質量,バネ定数は1.0
!!		PRINT "('p',I2,': ',F11.3)", i, eg0p
!		eg1p = eg1p + eg0p
!	END DO
	
	i=1
	eg0v = dt * rdx(i) ** 2 / 2.0		!質量は1.0
	eg0p = dt * (WIDTH*(i-1) - rx(i)) ** 2 / 2.0 !質量,バネ定数は1.0
	
	i=N_BODY
	eg1v = dt * rdx(i) ** 2 / 2.0		!質量は1.0
	eg1p = dt * (WIDTH*(i-1) - rx(i)) ** 2 / 2.0 !質量,バネ定数は1.0
	
	eg2  = dt * dx(2) ** 2 * m(2) / 2.0
	eg3  = dt * dx(3) ** 2 * m(3) / 2.0
	eg4  = dt * dx(4) ** 2 * m(4) / 2.0
	
	! DEBUG ! ちょっと黙っててね
!!PRINT "('L=',F8.3,' LL=',F8.3,' R=',F8.3,' RR=',F8.3,' Tot=',F8.3)", &
!!			eg0v+eg0p, eg0v+eg0p+eg2, &
!!			eg1v+eg1p, eg1v+eg1p+eg4, &
!!			eg0v+eg0p+eg1v+eg1p+eg2+eg3+eg4
	
	printEG = eg0v+eg0p+eg1v+eg1p+eg2+eg3+eg4	! 全エネルギーを返す
	return
END FUNCTION printEG

!***********************************************************************
! 振動子のエネルギーを求める
! x=位置の変位、dx=速度
REAL*8 FUNCTION VivEG(x, dx, dt)
IMPLICIT NONE
	REAL*8, INTENT(IN) :: x, dx, dt
	REAL*8 :: eg0v, eg0p
	
	eg0p = dt *  x ** 2 / 2.0	!質量,バネ定数は1.0
	eg0v = dt * dx ** 2 / 2.0	!質量は1.0
	VivEG = eg0p + eg0v
	return
END FUNCTION VivEG

!***********************************************************************
! エネルギーを平均化する
! 平均化の結果として速度rdxを補正する
SUBROUTINE AveEG_pool(rx, rdx, i, dt)
IMPLICIT NONE
	REAL*8, INTENT(IN) :: dt
	REAL*8, INTENT(INOUT) :: rx(:), rdx(:)
	INTEGER, INTENT(IN) :: i
	REAL*8 :: ave_eg, drx, eg, egv
	
	ave_eg = pool_get(i)	! 熱浴の中の１つからエネルギーを得る
							! i (熱浴の中のインデックス) に必然性は無い
	
	drx = WIDTH*(i-1) - rx(i)	! 変位
	eg = VivEG( drx, rdx(i), dt)
	eg = ave_eg - eg	! 平均との差異を求める!
!	PRINT "(I1,' : ',F8.3)", i, eg
	
	eg = eg / 10.0
	egv = eg + (dt * rdx(i) ** 2 / 2.0)
		! 差分の1/10を運動エネルギーに足し込む
	if ( egv > 0.0 &	! 正の場合に運動エネルギーを変える
&		.AND. ave_eg - eg > 0.0 ) then	! 熱浴がマイナスにならないように
		if ( rdx(i) >= 0.0 ) then	! 現在の速度の向きを考慮
			rdx(i) = sqrt( egv * 2.0 / dt )
		else
			rdx(i) = - sqrt( egv * 2.0 / dt )
		end if
		
		! 加えた分のエネルギーを熱浴から差し引く
		CALL pool_set( i, ave_eg - eg )
	end if
		! NOTE: ここのエネルギー受け渡しで誤差が累積し、
		! 全体のエネルギーが徐々に減少する傾向が見られた。
		! 誤差を減らすのが難しかったので、全体の補正を入れることにした
	
	return
END SUBROUTINE AveEG_pool

!***********************************************************************
! 衝突判定
! 返り値: 0=はずれ、1=あたり
LOGICAL FUNCTION isCol( x, next_x, i, j )
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, j
	REAL*8, INTENT(IN) :: x(:), next_x(:)
	REAL*8 :: now_x1, now_x2, next_x1, next_x2
	
	now_x1 = x(i)
	now_x2 = x(j)
	next_x1 = next_x(i)
	next_x2 = next_x(j)
	
	! 通常判定
	! now_は等しくても衝突しない、こうしないと繰り返し衝突が起こる
	! next_は等しければ衝突する
	if ( (now_x1 .gt. now_x2 .and. next_x1 .le. next_x2) &
&	.or. (now_x1 .lt. now_x2 .and. next_x1 .ge. next_x2) ) then
		isCol = .TRUE.
	else
		isCol = .FALSE.
	end if
	return
END FUNCTION isCol

!***********************************************************************
! 衝突処理
! dx[] : 衝突前後の速度 in-out
! m[]  : 質量 in
! i, j : 対象となる質点の番号 in
SUBROUTINE Collision( dx, m, i, j)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, j
	REAL*8, INTENT(INOUT) :: dx(:)
	REAL*8, INTENT(IN) :: m(:)
	REAL*8 :: dwork
	
!	PRINT "('Collision1 i=',I2,':',F7.2,' j=',I2,':',F7.2)", &
!&				i, dx(i), j, dx(j)
	dwork = ( dx(i) * m(i) - dx(i) * m(j) + 2.0 * dx(j) * m(j) ) &
&				/ ( m(i) + m(j) )
	dx(j) = ( 2.0 * dx(i) * m(i) - dx(j) * m(i) + dx(j) * m(j) ) &
&				/ ( m(i) + m(j) )
	dx(i) = dwork
!	PRINT "('Collision2 i=',I2,':',F7.2,' j=',I2,':',F7.2)", &
!&				i, dx(i), j, dx(j)
	return
END SUBROUTINE Collision

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

!**********************************************************************
! vの導関数
! 両端は -0.0, WIDTH で固定している
REAL*8 FUNCTION fv(t, x, v, i, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, N
	REAL*8, INTENT(IN) ::  t, x(i), v(i)
	! 両端の独立した単純な振動子
	fv = WIDTH*(i-1) - x(i)
	return
END FUNCTION fv

!**********************************************************************
! ルンゲ・クッタ法
SUBROUTINE Runge(t, x, v, xout, vout, dt, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: N
	REAL*8, INTENT(INOUT) :: t
	REAL*8, INTENT(IN) :: x(:), v(:)
	REAL*8, INTENT(OUT) :: xout(:), vout(:)
	REAL*8, INTENT(IN) :: dt
	real*8 :: h1
	real*8 :: kx(4,N), kv(4,N)
	real*8 :: kx1(N), x1(N), x2(N), x3(N)
	real*8 :: 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

!**********************************************************************
! 文字列(8)=>整数変換
INTEGER FUNCTION strtoi( num ) RESULT(i)
	character*8, intent(IN) :: num
	integer :: n, n1, k
	LOGICAL :: sgnflag = .false.
	
	k = len_trim(num)
	
	!先頭がマイナスだったら
	if (num(1:1) .eq. '-') then
		sgnflag = .true.
		n1 = 2
    else
		n1 = 1
	end if
	
	! 数字を１つずつ取り出して変換
	i = 0
	do n = n1, k
		i = i * 10 + ichar(num(n:n)) -48	! 48=ascii('0')
	end do
	
	!符号を整える
	if ( sgnflag ) then
		i = -i
	end if
END FUNCTION

END PROGRAM MaxDemon12
!*** END of FILE *******************************************************