// Fannel Ver2
val{
	jpn_1(default=0,min=-100000,max=100000,step=0,disp=0)
	jps_1(default=0,min=-100000,max=100000,step=0,disp=0)
	jpw_1(default=0,min=-100000,max=100000,step=0,disp=0)
	jpe_1(default=0,min=-100000,max=100000,step=0,disp=0)
	jpu_1(default=0,min=-100000,max=100000,step=0,disp=0)
	jpd_1(default=0,min=-100000,max=100000,step=0,disp=0)
	beam_1(default=0,min=0,max=100000,step=0,disp=0)
	col_1(default=#0000FF,step=0,disp=0)
	dmp_1(default=0.3,disp=0)
	spr_1(default=0.3,disp=0)

	jpn_2(default=0,min=-100000,max=100000,step=0,disp=0)
	jps_2(default=0,min=-100000,max=100000,step=0,disp=0)
	jpw_2(default=0,min=-100000,max=100000,step=0,disp=0)
	jpe_2(default=0,min=-100000,max=100000,step=0,disp=0)
	jpu_2(default=0,min=-100000,max=100000,step=0,disp=0)
	jpd_2(default=0,min=-100000,max=100000,step=0,disp=0)
	beam_2(default=0,min=0,max=100000,step=0,disp=0)
	col_2(default=#FF0000,step=0,disp=0)
	dmp_2(default=0.3,disp=0)
	spr_2(default=0.3,disp=0)

	jpowu_3(default=0,min=-100000,max=100000,step=0,disp=0)
	jpowf_3(default=0,min=-100000,max=100000,step=0,disp=0)
	jpowl_3(default=0,min=-100000,max=100000,step=0,disp=0)
	col_3(default=#00FF00,disp=0)
	dmp_3(default=1,disp=0)
	spr_3(default=1,disp=0)

	auto(default=0,min=0,max=2,step=2,disp=0)
}

key{
	7:auto(step=1)
}

body{
	core(name=c_1,color=col_1){
		n:jet(power=jpn_1,spring=spr_1,damper=dmp_1,color=col_1){
			s:trimf(angle=90,spring=spr_1,damper=dmp_1,color=col_1){
				w:jet(power=jpd_1,spring=spr_1,damper=dmp_1,color=col_1){
					e:trimf(angle=90,spring=spr_1,damper=dmp_1,color=col_1){
						n:jet(power=jpw_1,spring=spr_1,damper=dmp_1,color=col_1){
							n:arm(angle=90,power=beam_1,option=30000,spring=spr_1,damper=dmp_1,color=col_1){
							}
							n:arm(angle=-90,option=30000,spring=spr_1,damper=dmp_1,color=col_1){
							}
						}
						s:jet(power=jpe_1,spring=spr_1,damper=dmp_1,color=col_1){
							s:arm(angle=90,power=beam_1,option=30000,spring=spr_1,damper=dmp_1,color=col_1){
							}
							s:arm(angle=-90,option=30000,spring=spr_1,damper=dmp_1,color=col_1){
							}
						}
					}
				}
				e:jet(power=jpu_1,spring=spr_1,damper=dmp_1,color=col_1){
				}
			}
		}
		s:jet(power=jps_1,spring=spr_1,damper=dmp_1,color=col_1){
		}
//-----------------------------------------------------------------------------
		n:chip(name=c_2,color=col_2){
			n:jet(power=jpn_2,spring=spr_2,damper=dmp_2,color=col_2){
				s:trimf(angle=90,spring=spr_2,damper=dmp_2,color=col_2){
					w:jet(power=jpd_2,spring=spr_2,damper=dmp_2,color=col_2){
						e:trimf(angle=90,spring=spr_2,damper=dmp_2,color=col_2){
							n:jet(power=jpw_2,spring=spr_2,damper=dmp_2,color=col_2){
								n:arm(angle=90,power=beam_2,option=30000,spring=spr_2,damper=dmp_2,color=col_2){
								}
								n:arm(angle=-90,option=30000,spring=spr_2,damper=dmp_2,color=col_2){
								}
							}
							s:jet(power=jpe_2,spring=spr_2,damper=dmp_2,color=col_2){
								s:arm(angle=90,power=beam_2,option=30000,spring=spr_2,damper=dmp_2,color=col_2){
								}
								s:arm(angle=-90,option=30000,spring=spr_2,damper=dmp_2,color=col_2){
								}
							}
						}
					}
					e:jet(power=jpu_2,spring=spr_2,damper=dmp_2,color=col_2){
					}
				}
			}
			s:jet(power=jps_2,spring=spr_2,damper=dmp_2,color=col_2){
			}
		}
//-----------------------------------------------------------------------------
		s:jet(name=c_3,power=jpowu_3,color=col_3){
			n:jet(angle=90,power=jpowf_3,color=col_3,spring=spr_3,damper=dmp_3){
			}
			w:jet(angle=90,power=jpowl_3,color=col_3,spring=spr_3,damper=dmp_3){
			}
			e:jet(angle=90,power=-jpowl_3,color=col_3,spring=spr_3,damper=dmp_3){
			}
			s:jet(angle=90,power=-jpowf_3,color=col_3,spring=spr_3,damper=dmp_3){
				s:jet(angle=90,power=-jpowu_3,color=col_3,spring=spr_3,damper=dmp_3){
				}
			}
		}
//-----------------------------------------------------------------------------
	}
}
///////////////////////////////////////////////////////////////////////////////
Lua{
c_0=C_0
c_1=C_1
c_2=C_2
c_3=C_3
tgtx={}
tgty={}
tgtz={}
tgtx[c_1]=0
tgty[c_1]=0
tgtz[c_1]=0
tgtx[c_2]=0
tgty[c_2]=0
tgtz[c_2]=0
tgtx[c_3]=_X(c_3)
tgty[c_3]=_Y(c_3)
tgtz[c_3]=_Z(c_3)
cvelx,cvely,cvelz=0,0,0
tvelx,tvely,tvelz=0,0,0
ox,oy,oz=_OX(),_OY(),_OZ()
baseh=_Y(c_3)
mode=0

-- キューブ重心位置 -----------------------------------------------------------
function cube_gpos(cn)
	local gx,gy,gz=0,0,0
	local c
	for i=0,5,1 do
		c=cn+i
		gx=gx+_X(c)
		gy=gy+_Y(c)
		gz=gz+_Z(c)
	end
	gx=gx/6
	gy=gy/6
	gz=gz/6
	return gx,gy,gz
end

-- キューブターゲット座標設定 -------------------------------------------------
function cube_target(cn)
	local gx,gy,gz=cube_gpos(cn)
	local lx,ly,lz
	if(mode==0) then
		lx=tgtx[cn]-gx
		ly=tgty[cn]-gy
		lz=tgtz[cn]-gz
		local dst=math.sqrt(lx^2+ly^2+lz^2)
		if(dst<50)or(math.mod(_TICKS(),1800)==0) then
			tgtx[cn]=math.floor(math.random(1000))-500
			tgty[cn]=math.floor(math.random(101))+10+baseh
			tgtz[cn]=math.floor(math.random(1000))-500
		end
	else
		tgtx[cn]=_OX()
		tgty[cn]=_OY()+2
		tgtz[cn]=_OZ()
	end
	local k=10-mode*9
	lx=tgtx[cn]-gx-cvelx*k
	ly=tgty[cn]-gy-cvely*k
	lz=tgtz[cn]-gz-cvelz*k
	return lx,ly,lz
end

-- キューブジェット出力 -------------------------------------------------------
function cube_power(cn)
	local jpowu,jpowf,jpowl
	local r,c

	local lx,ly,lz=cube_target(cn)
	lx,ly,lz=getlpos(lx,ly,lz,cn)
	local dst=math.sqrt(lx^2+ly^2+lz^2)
	local pow=math.min(dst*5000,50000)

	r=math.sqrt(lx*lx+lz*lz)
	c=math.atan2(r,ly)
	jpowu= pow*math.cos(c)
	r=math.sqrt(lx*lx+ly*ly)
	c=math.atan2(r,lz)
	jpowf=-pow*math.cos(c)
	r=math.sqrt(ly*ly+lz*lz)
	c=math.atan2(r,lx)
	jpowl= pow*math.cos(c)

	-- 重力補正
	lx,ly,lz=getlpos(0,1,0,cn)
	pow=gp
	r=math.sqrt(lx*lx+lz*lz)
	c=math.atan2(r,ly)
	jpowu=jpowu+pow*math.cos(c)
	r=math.sqrt(lx*lx+ly*ly)
	c=math.atan2(r,lz)
	jpowf=jpowf-pow*math.cos(c)
	r=math.sqrt(ly*ly+lz*lz)
	c=math.atan2(r,lx)
	jpowl=jpowl+pow*math.cos(c)

	return jpowu,jpowf,jpowl
end

-- キューブ重心速度 -----------------------------------------------------------
function cube_gvel(cn)
	local c1=cn+1
	local c2=cn+2
	local c3=cn+3
	local c4=cn+4
	local c5=cn+5
	local gx=(_VX(cn)+_VX(c1)+_VY(c2)-_VY(c3)+_VX(c4)+_VX(c5))/6
	local gy=(_VY(cn)+_VZ(c1)-_VX(c2)+_VX(c3)-_VZ(c4)-_VY(c5))/6
	local gz=(_VZ(cn)-_VY(c1)+_VZ(c2)+_VZ(c3)+_VY(c4)-_VZ(c5))/6
	return gx,gy,gz
end

-- キューブ並進速度 -----------------------------------------------------------
function cube_vel(cn)
	local gx,gy,gz=cube_gvel(cn)
	local tvx,tvy,tvz=getwpos(gx,gy,gz,cn)
	return tvx,tvy,tvz
end

-- ターゲット並進速度 ---------------------------------------------------------
function target_vel(mode)
	local tvx,tvy,tvz
	local ox1,oy1,oz1=_OX(),_OY(),_OZ()
	if(mode==0) then
		tvx,tvy,tvz=cvelx,cvely,cvelz
	else
		tvx=(ox1-ox)*30
		tvy=(oy1-oy)*30
		tvz=(oz1-oz)*30
	end
	ox,oy,oz=ox1,oy1,oz1
	return tvx,tvy,tvz
end

-- len3 -----------------------------------------------------------------------
function math.len3(x,y,z,cn)
	x=x-_X(cn)
	y=y-_Y(cn)
	z=z-_Z(cn)
	local d=math.sqrt(x^2+y^2+z^2)
	return d
end

-- 角度補正 -------------------------------------------------------------------
function angle_hosei(a)
	while a> math.pi do a=a-math.pi*2 end
	while a<-math.pi do a=a+math.pi*2 end
	return a
end

-- 出力リミッタ ---------------------------------------------------------------
function limiter(jp,limit)
	jp=math.min(limit,math.max(-limit,jp))
	return jp
end

-- GETWPOS --------------------------------------------------------------------
function getwpos(lx,ly,lz,cn)
	-- Z軸回転を加算する（AZだけ回転させる）
	ly,lx=ly*math.cos(_EZ(cn))+lx*math.sin(_EZ(cn)),lx*math.cos(_EZ(cn))-ly*math.sin(_EZ(cn))
	-- X軸回転を加算する（AXだけ回転させる）
	lz,ly=lz*math.cos(_AX(cn))+ly*math.sin(_AX(cn)),ly*math.cos(_AX(cn))-lz*math.sin(_AX(cn))
	-- Y軸回転を加算する（AYだけ回転させる）
	lz,lx=lz*math.cos(_AY(cn))-lx*math.sin(_AY(cn)),lx*math.cos(_AY(cn))+lz*math.sin(_AY(cn))
	return lx,ly,lz
end

-- GETLPOS --------------------------------------------------------------------
function getlpos(lx,ly,lz,cn)
	-- Y軸回転を差し引く（-AYだけ回転させる）
	lz,lx=lz*math.cos(_AY(cn))+lx*math.sin(_AY(cn)),lx*math.cos(_AY(cn))-lz*math.sin(_AY(cn))
	-- X軸回転を差し引く（-AXだけ回転させる）
	lz,ly=lz*math.cos(_AX(cn))-ly*math.sin(_AX(cn)),ly*math.cos(_AX(cn))+lz*math.sin(_AX(cn))
	-- Z軸回転を差し引く（-AZだけ回転させる）
	ly,lx=ly*math.cos(_EZ(cn))-lx*math.sin(_EZ(cn)),lx*math.cos(_EZ(cn))+ly*math.sin(_EZ(cn))
	return lx,ly,lz
end

-- ジェット出力並進制御 -------------------------------------------------------
function jetlshift(lx,ly,lz,p,cn)
	local jp={}
	local br
	lx,ly,lz=getlpos(lx,ly,lz,cn)
	if math.abs(ly)<15 then br=0.2 else br=0 end
	for i=1,2,1 do jp[i]=p*(ly-_VY(cn)*br) end
	if math.abs(lx)<15 then br=0.2 else br=0 end
	for i=3,4,1 do jp[i]=p*(lx-_VX(cn)*br) end
	if math.abs(lz)<15 then br=0.2 else br=0 end
	for i=5,6,1 do jp[i]=p*(lz-_VZ(cn)*br) end
	for i=1,6,1 do jp[i]=limiter(jp[i],150000) end
	return jp[1],jp[2],jp[3],jp[4],jp[5],jp[6]
end

-- ジェット出力回転制御 -------------------------------------------------------
function jetlroll(rx,ry,rz,cn)
	local jp={}
	local p=30000
	local br=0.1
	jp[1]=p*(rx-_WX(cn)*br)
	jp[2]=-jp[1]
	jp[3]=-p*(rz-_WZ(cn)*br*2)
	jp[4]=-jp[3]
	jp[5]=-p*(ry-_WY(cn)*br*2)
	jp[6]=-jp[5]
	for i=1,6,1 do jp[i]=limiter(jp[i],60000) end
	return jp[1],jp[2],jp[3],jp[4],jp[5],jp[6]
end

-- 重力補正 -------------------------------------------------------------------
function gravity_hosei(cn,g)
	local jp={}
	jp[1],jp[2],jp[3],jp[4],jp[5],jp[6]=jetlshift(0,1,0,g,cn)
	return jp[1],jp[2],jp[3],jp[4],jp[5],jp[6]
end

-- 並進目標 -----------------------------------------------------------------
function target_p(cx,cy,cz,r,h,cn)
	local lx,ly,lz
	local k=0.5
	if(math.random(100)<4) then
		tgtx[cn]=math.random()*2*r-r
		tgty[cn]=math.random()*2*h-h
		tgtz[cn]=math.random()*2*r-r
	end
	lx=tgtx[cn]+cx+tvelx*k
	ly=tgty[cn]+cy+tvely*k
	if(ly<baseh+10) then ly=baseh+10 end
	lz=tgtz[cn]+cz+tvelz*k
	local dst=math.len3(lx,ly,lz,cn)
	lx=lx-_X(cn)
	ly=ly-_Y(cn)
	lz=lz-_Z(cn)
	return lx,ly,lz,dst
end

-- 回転目標 ----------------------------------------------------------------
function target_r(lx,ly,lz,cn)
	lx,ly,lz=lx-_X(cn),ly-_Y(cn),lz-_Z(cn)
	local s=150	-- 弾速[m/s]
	local vx,vy,vz=tvelx/2,tvely/2,tvelz/2
	local a=vx^2+vy^2+vz^2-s^2
	local b=2*(lx*vx+ly*vy+lz*vz)
	local c=lx^2+ly^2+lz^2
	local d=b^2-4*a*c
	local k
	if(a==0)and(b~=0) then
		k=-c/b
	elseif(d>=0) then
		k=(-b-math.sqrt(d))/2/a
	else
		k=0
	end

	local rx0=math.atan2(ly,math.sqrt(lx^2+lz^2))-_AX(cn)
	local ry0=math.atan2(-lx,-lz)-_AY(cn)
	rx0=angle_hosei(rx0)
	ry0=angle_hosei(ry0)
	lx=lx+vx*k
	ly=ly+vy*k
	lz=lz+vz*k
	local rx=math.atan2(ly,math.sqrt(lx^2+lz^2))-_AX(cn)
	local ry=math.atan2(-lx,-lz)-_AY(cn)
	local rz=-_EZ(cn)
	rx=angle_hosei(rx)
	ry=angle_hosei(ry)
	return rx,ry,rz,rx0,ry0
end

-- ジェット出力 ---------------------------------------------------------------
function jet_power(cn)
	local jp={0,0,0,0,0,0}
	local js={}
	local tx,ty,tz
	local beam=0

	js[1],js[2],js[3],js[4],js[5],js[6]=gravity_hosei(cn,g)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	if(mode==0) then
		tx,ty,tz=_X(c_3),_Y(c_3),_Z(c_3)
	else
		tx,ty,tz=_OX(),_OY(),_OZ()
	end

	local lx,ly,lz,dst=target_p(tx,ty,tz,30,30,cn)
	js[1],js[2],js[3],js[4],js[5],js[6]=jetlshift(lx,ly,lz,10000,cn)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	local rx,ry,rz,rx0,ry0=target_r(tx,ty,tz,cn)
	js[1],js[2],js[3],js[4],js[5],js[6]=jetlroll(rx,ry,rz,cn)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	local a=math.rad(4)
	if(math.abs(rx0)<a)and(math.abs(ry0)<a) then
		beam=1000000
	end

	return jp[1],jp[2],jp[3],jp[4],jp[5],jp[6],beam
end

-- 重力補正係数チェック用（ファンネル） ---------------------------------------
function check1(cn,g)
	local jp={0,0,0,0,0,0}
	local js={}
	local tx,ty,tz

	js[1],js[2],js[3],js[4],js[5],js[6]=gravity_hosei(cn,g)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	local lx,ly,lz=-_X(cn),0,-_Z(cn)
	js[1],js[2],js[3],js[4],js[5],js[6]=jetlshift(lx,ly,lz,10000,cn)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	local rx,ry,rz=-_AX(cn),-_AY(cn),-_EZ(cn)
	js[1],js[2],js[3],js[4],js[5],js[6]=jetlroll(rx,ry,rz,cn)
	for i=1,6,1 do
		jp[i]=jp[i]+js[i]
	end

	return jp[1],jp[2],jp[3],jp[4],jp[5],jp[6],0
end

-- 重力補正係数チェック用（キューブ） -----------------------------------------
function check2(cn)
	local jpowu,jpowf,jpowl=0,0,0
	local r,c

	local lx,ly,lz=-_X(c_3),0,-_Z(c_3)
	lx,ly,lz=getlpos(lx,ly,lz,cn)
	local dst=math.sqrt(lx^2+ly^2+lz^2)
	local pow=dst*100

	r=math.sqrt(lx*lx+lz*lz)
	c=math.atan2(r,ly)
	jpowu= pow*math.cos(c)
	r=math.sqrt(lx*lx+ly*ly)
	c=math.atan2(r,lz)
	jpowf=-pow*math.cos(c)
	r=math.sqrt(ly*ly+lz*lz)
	c=math.atan2(r,lx)
	jpowl= pow*math.cos(c)

	-- 重力補正
	lx,ly,lz=getlpos(0,1,0,cn)
	pow=g3
	r=math.sqrt(lx*lx+lz*lz)
	c=math.atan2(r,ly)
	jpowu=jpowu+pow*math.cos(c)
	r=math.sqrt(lx*lx+ly*ly)
	c=math.atan2(r,lz)
	jpowf=jpowf-pow*math.cos(c)
	r=math.sqrt(ly*ly+lz*lz)
	c=math.atan2(r,lx)
	jpowl=jpowl+pow*math.cos(c)

	return jpowu,jpowf,jpowl
end

-- メイン ---------------------------------------------------------------------
gcheck=0
function main()
	local str

	if(_TICKS()<2) then
		g1,g2,g3=9453.801231,9453.801231,6488.270889
		m1,m2,m3=0,0,0
		hcheck1=_Y(c_1)
		hcheck2=_Y(c_2)
		hcheck3=_Y(c_3)
		for i=c_1,c_2-1,1 do
			m1=m1+_M(i)
		end
		for i=c_2,c_3-1,1 do
			m2=m2+_M(i)
		end
		for i=c_3,_CHIPS()-1,1 do
			m3=m3+_M(i)
		end
		g=m1*9.80665*6/2
		gp=m3*9.80665*6/2
		_SPLIT(c_2)
		_SPLIT(c_3)
	end

	if(gcheck>0) then
		cvelx,cvely,cvelz=cube_vel(c_3)
		local h1=_Y(c_1)-hcheck1
		g1=g1-h1
		hcheck1=_Y(c_1)
		JPN_1,JPS_1,JPU_1,JPD_1,JPW_1,JPE_1,BEAM_1=check1(c_1,g1)

		local h2=_Y(c_2)-hcheck2
		g2=g2-h2
		hcheck2=_Y(c_2)
		JPN_2,JPS_2,JPU_2,JPD_2,JPW_2,JPE_2,BEAM_2=check1(c_2,g2)

		local ghx,ghy,ghz=cube_gpos(c_3)
		local h3=ghy-hcheck3
		g3=g3-h3/10
		hcheck3=ghy
		JPOWU_3,JPOWF_3,JPOWL_3=check2(c_3)
	else
		if(AUTO==1) then mode=math.mod(mode+1,2) end
		if(_OY()==-1000000) then mode=0 end
		cvelx,cvely,cvelz=cube_vel(c_3)
		tvelx,tvely,tvelz=target_vel(mode)

		if(mode==0) then
			str="Chase Target-Cube Mode"
		else
			str="Chase Target-Ball Mode"
		end

		JPN_1,JPS_1,JPU_1,JPD_1,JPW_1,JPE_1,BEAM_1=jet_power(c_1)
		JPN_2,JPS_2,JPU_2,JPD_2,JPW_2,JPE_2,BEAM_2=jet_power(c_2)
		JPOWU_3,JPOWF_3,JPOWL_3=cube_power(c_3)
	end

	local vel=math.sqrt(_VX(c_1)^2+_VY(c_1)^2+_VZ(c_1)^2)
	out(0,"FPS=",_FPS(),", CHIPS=",_CHIPS(),", WEIGHT=",_WEIGHT(),"kg, SPEED=",vel*3.6,"km/h")
	if(gcheck==0) then
		out(1,str)
		out(2,"A:Target Change (Cube/Ball)")
	else
		out(1,"X=",_X(c_1),", Y=",_Y(c_1),", Z=",_Z(c_1))
		out(2,"AX=",math.deg(_AX(c_1)),", AY=",math.deg(_AY(c_1)),", EZ=",math.deg(_EZ(c_1)))
		out(3,"VX=",_VX(c_1),", VY=",_VY(c_1),", VZ=",_VZ(c_1))
		out(4,"WX=",_WX(c_1),", WY=",_WY(c_1),", WZ=",_WZ(c_1))
		out(5,"TVX=",tvelx,", TVY=",tvely,", TVZ=",tvelz)
		out(6,"M1=",m1,", M2=",m2,", M3=",m3)
		out(7,"g1=",g1,", g2=",g2,", g3=",g3)
		out(8,"g1,g2=",m1*9.80665*6/2,", g3=",m3*9.80665*6/2)
		out(9,"baseh=",baseh)
	end
end
}