!***********************************************************************
!	振動子と粒子の衝突
!	両端を２つの振動子で挟み、その温度(エネルギー)を等しく保つ
!	Date: 2007/02/14
!	g95 MaxDemon05.F90 -o3 -Wno=165 -Wall -O -o MaxDemon05 -Wl,--subsystem,console -lGrWin -mwindows
PROGRAM MaxDemon05
IMPLICIT NONE

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

	! 物体の数、1=等温壁、2=ボール、3=隔壁、4=ボール、5=等温壁
	INTEGER, PARAMETER :: N_BALL = 5
	! 振動子の数（左右で独立、連動してはいない）
	INTEGER, PARAMETER :: N_BODY = 2
	! 画面サイズ
	INTEGER, PARAMETER :: WIDTH = 500, HEIGHT = 300
	! 壁の位置
	REAL, PARAMETER :: W1POS = 0.0
	! 描画する物体の高さ、大きさ
	REAL, PARAMETER :: yy=200.0, rr=8.0
	
	REAL, PARAMETER :: rx0 = 10.0	! 初期振動子の振幅
	REAL :: x(1:N_BALL)  = (/rx0, 125.0, 250.0, 375.0, real(WIDTH)-rx0/)
	REAL :: next_x(1:N_BALL)	! 1ステップ先の位置
	REAL :: dx(1:N_BALL) = (/0.0, 10.0, 0.0, 10.0, 0.0/)
	REAL :: m(1:N_BALL)  = (/1.0, 1.0, 5.0, 1.0, 1.0/)
	REAL :: t=0.0
!	REAL :: dt=0.1	! 計差精度粗くしてみる
	REAL :: dt=0.05 ! 計算精度標準
!	REAL :: dt=0.008	!高精度
	
	! 連成振動子の位置と速度
	REAL :: rx(1:N_BODY) = (/rx0, real(WIDTH)-rx0/)
	REAL :: rdx(1:N_BODY) = (/0.0, 0.0/)
	REAL :: next_rx(1:N_BODY)
	REAL :: next_rdx(1:N_BODY)
	REAL, PARAMETER :: yy2 = 100.0
	REAL :: ave_eg1, ave_eg2	!振動子の平均エネルギー
	
	! ワープゾーンの位置
	INTEGER, PARAMETER :: P1FROM =  50, P1TO = 100
	INTEGER, PARAMETER :: P2FROM = 225, P2TO = 275
	
	! ワープ用作業変数
	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 :: i, iter
#ifndef NOGR
	INTEGER :: IR, NOG=1	!GWライブラリ関連
#endif
	INTEGER, PARAMETER :: MAXITER = 100

! BEGIN
#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)
	
	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)
		
		! エネルギーを平均化する
		CALL AveEG(rx, rdx, 1, ave_eg1, dt)
		CALL AveEG(rx, rdx, 2, ave_eg2, dt)
		
		!ワープ処理 -- 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, x(i)-rr, yy-30.0, x(i)+rr, yy+30.0, 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, x(i)-rr, yy-rr, x(i)+rr, 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, rx(i)-rr, yy2-rr, rx(i)+rr, 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
	!!	WRITE(*,*) ave_eg1, ave_eg2	! 平均エネルギー
	!!	CALL printEG(rx, rdx, dx, m, dt)	! エネルギーの表示
		
		! ヒストグラムをとる
	!!	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
		! 1000回でシミュレーションを打ち切る
	!	IF (tothistcnt > 3000) THEN
		IF (totwarpcnt > 1000) THEN
			EXIT
		END IF
#endif
	END DO
	
#ifndef NOGR
	CALL GWSETBK(IR, 1)
	CALL GWREFRESH(IR)
	CALL GWQUIT(IR)
#endif

CONTAINS

!***********************************************************************
! エネルギーの表示
! 両端は -0.0, WIDTH で固定している
SUBROUTINE printEG(rx, rdx, dx, m, dt)
IMPLICIT NONE
	REAL, INTENT(IN) :: rx(:), rdx(:), dx(:), m(:), dt
	REAL :: 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
	
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
	
	return
END SUBROUTINE printEG

!***********************************************************************
! 振動子のエネルギーを求める
! x=位置の変位、dx=速度
REAL FUNCTION VivEG(x, dx, dt)
IMPLICIT NONE
	REAL, INTENT(IN) :: x, dx, dt
	REAL :: 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(rx, rdx, i, ave_eg, dt)
IMPLICIT NONE
	REAL, INTENT(IN) :: ave_eg, dt
	REAL, INTENT(INOUT) :: rx(:), rdx(:)
	INTEGER, INTENT(IN) :: i
	REAL :: drx, eg, egv
	
	drx = WIDTH*(i-1) - rx(i)	! 変位
	eg = VivEG( drx, rdx(i), dt)
	eg = ave_eg - eg	! 平均との差異を求める!
!	PRINT "(I1,' : ',F8.3)", i, eg
	
	egv = eg / 10.0 + (dt * rdx(i) ** 2 / 2.0)
		! 差分の1/10を運動エネルギーに足し込む
	if ( egv > 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
	end if
	
	return
END SUBROUTINE AveEG

!***********************************************************************
! 衝突判定
! 返り値: 0=はずれ、1=あたり
LOGICAL FUNCTION isCol( x, next_x, i, j )
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, j
	REAL, INTENT(IN) :: x(:), next_x(:)
	REAL :: 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, INTENT(INOUT) :: dx(:)
	REAL, INTENT(IN) :: m(:)
	REAL :: 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 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の導関数
! 両端は -0.0, WIDTH で固定している
REAL FUNCTION fv(t, x, v, i, N)
IMPLICIT NONE
	INTEGER, INTENT(IN) :: i, N
	REAL, 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, 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 MaxDemon05
!*** END of FILE *******************************************************