//=====================================================================//
/*!	@file
	@brief	物理関係・クラス
	@author	平松邦仁 (hira@rvf-rc45.net)
*/
//=====================================================================//
#include "physics.hpp"
#include <boost/foreach.hpp>
#include "gl_fw/IGLcore.hpp"
#include <boost/foreach.hpp>
#include "utils/quat.hpp"

#if 0
void* btAlignedAllocInternal(size_t size, int alignment)
{
	return malloc(size);
}

void btAlignedFreeInternal(void* ptr)
{
	free(ptr);
}
#endif

namespace bt {

	btVector3 physics::get_ray_to_(const gl::camera& cam, int x, int y)
	{
#if 0
		if(m_ortho) {
			btScalar aspect;
			btVector3 extents;
			aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
			extents.setValue(aspect * 1.0f, 1.0f,0);

			extents *= m_cameraDistance;
			btVector3 lower = m_cameraTargetPosition - extents;
			btVector3 upper = m_cameraTargetPosition + extents;

			btScalar u = x / btScalar(m_glutScreenWidth);
			btScalar v = (m_glutScreenHeight - y) / btScalar(m_glutScreenHeight);

			btVector3	p(0,0,0);
			p.setValue((1.0f - u) * lower.getX() + u * upper.getX(),(1.0f - v) * lower.getY() + v * upper.getY(),m_cameraTargetPosition.getZ());
			return p;
		}
#endif
		const vtx::fvtx& v = cam.get_eye();
		btVector3 rayFrom(v.x, v.y, v.z);
		vtx::fvtx d = cam.get_target() - cam.get_eye();
		btVector3 rayForward(d.x, d.y, d.z);
		rayForward.normalize();
		float farPlane = cam.get_far();
		rayForward *= farPlane;

		const vtx::fvtx& up = cam.get_up();
		btVector3 horizontal = rayForward.cross(btVector3(up.x, up.y, up.z));
		horizontal.normalize();
		btVector3 vertical = horizontal.cross(rayForward);
		vertical.normalize();

		float tanfov = tanf(0.5f * cam.get_fov() * vtx::g_deg2rad_f);
		horizontal *= 2.0f * farPlane * tanfov;
		vertical   *= 2.0f * farPlane * tanfov;

		btScalar aspect(cam.get_aspect());

		horizontal *= aspect;

		btVector3 rayToCenter = rayFrom + rayForward;
		const vtx::fpos& size = cam.get_size();
		btVector3 dH = horizontal * 1.0f / size.x;
		btVector3 dV = vertical   * 1.0f / size.y;

		btVector3 rayTo = rayToCenter - 0.5f * horizontal + 0.5f * vertical;
		rayTo += btScalar(x) * dH;
		rayTo -= btScalar(y) * dV;

		return rayTo;
	}

	void physics::picking_body_(const gl::camera& cam, const btVector3& ray_to)
	{
		btVector3 ray_from;
		const vtx::fvtx& cp = cam.get_eye();
		ray_from = btVector3(cp.x, cp.y, cp.z);

		btCollisionWorld::ClosestRayResultCallback ray_callback(ray_from, ray_to);
		world_->rayTest(ray_from, ray_to, ray_callback);
		if(ray_callback.hasHit()) {
#if 0
			btRigidBody* body = btRigidBody::upcast(ray_callback.m_collisionObject);
			if(body) {
				//other exclusions?
				if(!(body->isStaticObject() || body->isKinematicObject())) {
					picked_body_ = body;
					body->setActivationState(DISABLE_DEACTIVATION);
					const btVector3& pick_pos = ray_callback.m_hitPointWorld;
					// std::cout
					//     << boost::format("pickPos=%f,%f,%f\n")
							% pickPos.getX() % pickPos.getY() % pickPos.getZ();
					const btVector3& local_pivot = body->getCenterOfMassTransform().inverse() * pick_pos;
					if(use_6dof_) {
						btTransform tr;
						tr.setIdentity();
						tr.setOrigin(local_pivot);
						btGeneric6DofConstraint* dof = new btGeneric6DofConstraint(*body, tr, false);
						dof->setLinearLowerLimit(btVector3(0, 0, 0));
						dof->setLinearUpperLimit(btVector3(0, 0, 0));
						dof->setAngularLowerLimit(btVector3(0, 0, 0));
						dof->setAngularUpperLimit(btVector3(0, 0, 0));
						world_->addConstraint(dof);
						pick_constraint_ = dof;
						for(int i = 0; i < 6; ++i) {
							dof->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, i);
							dof->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, i);
						}
					} else {
						btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, local_pivot);
						world_->addConstraint(p2p);
						pick_constraint_ = p2p;
///						p2p->m_setting.m_impulseClamp = mousePickClamping;
						//very weak constraint for picking
						p2p->m_setting.m_tau = 0.001f;
/*
						p2p->setParam(BT_CONSTRAINT_CFM,0.8,0);
						p2p->setParam(BT_CONSTRAINT_CFM,0.8,1);
						p2p->setParam(BT_CONSTRAINT_CFM,0.8,2);
						p2p->setParam(BT_CONSTRAINT_ERP,0.1,0);
						p2p->setParam(BT_CONSTRAINT_ERP,0.1,1);
						p2p->setParam(BT_CONSTRAINT_ERP,0.1,2);
						*/
					}
					use_6dof_ = !use_6dof_;
					//save mouse position for dragging
					last_picking_pos_ = ray_to;
///					gHitPos = pick_pos;
					last_picking_dist_ = (pick_pos - ray_from).length();
				}
			}
#endif
		}
	}

	void physics::moveing_body_(const gl::camera& cam, const btVector3& ray)
	{
		if(pick_constraint_) {
			//move the constraint pivot
			if(pick_constraint_->getConstraintType() == D6_CONSTRAINT_TYPE) {
				btGeneric6DofConstraint* pick = static_cast<btGeneric6DofConstraint*>(pick_constraint_);
				if(pick) {
					//keep it at the same picking distance
					btVector3 new_pivot;
//					if (m_ortho)
//					{
//						const btVector3& old_pivot = pick->getFrameOffsetA().getOrigin();
//						newPivot = old_pivot;
//						newPivot.setX(ray.getX());
//						newPivot.setY(ray.getY());
//					} else
					{
						const vtx::fvtx& v = cam.get_eye();
						btVector3 ray_from(v.x, v.y, v.z);
						btVector3 dir = ray - ray_from;
						dir.normalize();
						dir *= last_picking_dist_;
						new_pivot = ray_from + dir;
					}
					pick->getFrameOffsetA().setOrigin(new_pivot);
				}
			} else {
				btPoint2PointConstraint* pick = static_cast<btPoint2PointConstraint*>(pick_constraint_);
				if(pick) {
					//keep it at the same picking distance
					btVector3 new_pivot;
//					if (m_ortho)
//					{
//						btVector3 oldPivotInB = pick->getPivotInB();
//						newPivotB = oldPivotInB;
//						newPivotB.setX(ray.getX());
//						newPivotB.setY(ray.getY());
//					} else
					{
						const vtx::fvtx& v = cam.get_eye();
						btVector3 ray_from(v.x, v.y, v.z);
						btVector3 dir = ray - ray_from;
						dir.normalize();
						dir *= last_picking_dist_;
						new_pivot = ray_from + dir;
					}
					pick->setPivotB(new_pivot);
				}
			}
		}
	}

	void physics::remove_picking_constraint_()
	{
		if(world_ && picked_body_ && pick_constraint_) {
			world_->removeConstraint(pick_constraint_);
			delete pick_constraint_;
			pick_constraint_ = 0;

			picked_body_->forceActivationState(ACTIVE_TAG);
			picked_body_->setDeactivationTime(0.0f);
			picked_body_ = 0;
		}
	}

	void physics::render_world_(int pass)
	{
		btScalar m[16];
		btMatrix3x3 rot;
		rot.setIdentity();
		int num = world_->getNumCollisionObjects();
		btVector3 wireColor(1, 0, 0);
		for(int i = 0; i < num; ++i) {
			btCollisionObject* cobj = world_->getCollisionObjectArray()[i];
			btRigidBody* body = btRigidBody::upcast(cobj);
			if(body && body->getMotionState()) {
				btDefaultMotionState* state = (btDefaultMotionState*)body->getMotionState();
				state->m_graphicsWorldTrans.getOpenGLMatrix(m);
				rot = state->m_graphicsWorldTrans.getBasis();
			} else {
				cobj->getWorldTransform().getOpenGLMatrix(m);
				rot = cobj->getWorldTransform().getBasis();
			}

			btVector3 wireColor(1.0f, 1.0f, 0.5f); //wants deactivation
			if(i & 1) wireColor = btVector3(0.0f, 0.0f, 1.0f);

			// color differently for active, sleeping, wantsdeactivation states
			if(cobj->getActivationState() == 1) {
				if (i & 1) {
					wireColor += btVector3 (1.f,0.f,0.f);
				} else {
					wireColor += btVector3 (.5f,0.f,0.f);
				}
			}
			if(cobj->getActivationState() == 2) {	//ISLAND_SLEEPING
				if(i & 1) {
					wireColor += btVector3 (0.f,1.f, 0.f);
				} else {
					wireColor += btVector3 (0.f,0.5f,0.f);
				}
			}

			btVector3 aabbMin,aabbMax;
			world_->getBroadphase()->getBroadphaseAabb(aabbMin, aabbMax);
			aabbMin -= btVector3(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
			aabbMax += btVector3(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);

///			std::cout << boost::format("aabbMin=(%f,%f,%f)\n")
///				% aabbMin.getX() % aabbMin.getY() % aabbMin.getZ();
///			std::cout << boost::format("aabbMax=(%f,%f,%f)\n")
///				% aabbMax.getX() % aabbMax.getY() % aabbMax.getZ();
///			world_->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1));

			if (!(debug_mode_ & btIDebugDraw::DBG_DrawWireframe)) {
				switch(pass)
				{
				case 0:
					shape_drawer_.drawOpenGL(m, cobj->getCollisionShape(),
						wireColor, debug_mode_, aabbMin, aabbMax);
					break;
				case 1:
					shape_drawer_.drawShadow(m, sun_direction_ * rot,
						cobj->getCollisionShape(), aabbMin, aabbMax);
					break;
				case 2:
					shape_drawer_.drawOpenGL(m, cobj->getCollisionShape(),
						wireColor * btScalar(0.3f), 0, aabbMin, aabbMax);
					break;
				}
			}
		}
	}

	btRigidBody* physics::new_body_(btCollisionShape* shape, const vtx::fvtx& pos, const vtx::fvtx& dir)
	{
		btTransform transform;
		transform.setIdentity();

		btScalar mass(current_.mass_);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 inertia(0, 0, 0);
		if (isDynamic) {
			shape->calculateLocalInertia(mass, inertia);
		}
		transform.setOrigin(btVector3(pos.x, pos.y, pos.z));
		qtx::fquat qu;
		qu.look_rotation(dir, vtx::fvtx(0.0f, 0.0f, 1.0f));
		btQuaternion q(btScalar(qu.x), btScalar(qu.y), btScalar(qu.z), btScalar(qu.t));
		transform.setRotation(q);

		// using motionstate is recommended,
		// it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* state = new btDefaultMotionState(transform);
		btRigidBody::btRigidBodyConstructionInfo info(mass, state, shape, inertia);
		return new btRigidBody(info);
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	初期化
	*/
	//-----------------------------------------------------------------//
	void physics::initialize()
	{
		shape_drawer_.enable_texture();

		config_ = new btDefaultCollisionConfiguration();
		dispatcher_ = new	btCollisionDispatcher(config_);
		broadphase_ = new btDbvtBroadphase();
		solver_ = new btSequentialImpulseConstraintSolver;
		world_ = new btDiscreteDynamicsWorld(dispatcher_, broadphase_, solver_, config_);

		world_->setGravity(btVector3(0.0f, 0.0f, -9.8f));
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	地面剛体生成
	*/
	//-----------------------------------------------------------------//
	void physics::create_ground_body()
	{
		current_.mass_ = 0.0f;	/// 重さ０（固定）
		create_box(vtx::fvtx(0.0f, 0.0f, -2.5f), vtx::fvtx(7.5f, 7.5f, 0.05f));

		current_.mass_ = 1.0f;
		create_sphere(vtx::fvtx(0.0f, 0.0f, 10.0f), 0.5f);

		// 実験用剛体～
		if(0) {
			btRigidBody* bodyA;
			btRigidBody* bodyB;
			vtx::fvtx posA(0.0f, 0.0f, 1.0f);
			vtx::fvtx posB(0.0f, 0.0f, 1.0f + 2.0f + 0.2f * 2);
			{
				current_.mass_ = 0.0f;
				btCollisionShape* cshape = new btCapsuleShapeZ(0.2f, 2.0f);
				shapes_.push_back(cshape);
				vtx::fvtx dir(0.0f, 0.0f, 1.0f);
				bodyA = new_body_(cshape, posA, dir);
				bodyA->setDamping(0.15f, 0.15f);
				world_->addRigidBody(bodyA);
			}
			{
				current_.mass_ = 1.0f;
				btCollisionShape* cshape = new btCapsuleShapeZ(0.2f, 2.0f);
				shapes_.push_back(cshape);
				vtx::fvtx dir(0.0f, 0.0f, 1.0f);
				bodyB = new_body_(cshape, posB, dir);
				bodyB->setDamping(0.15f, 0.15f);
				world_->addRigidBody(bodyB);
			}

			create_joint(bodyA, bodyB, 1.2f, -1.2f);
		}

	}

	//-----------------------------------------------------------------//
	/*!
		@brief	箱作成
		@param[in]	org		起点
		@param[in]	size	サイズ
		@param[in]	dir		方向（省略するとZ上向き）
		@return RigidBody のポインター
	*/
	//-----------------------------------------------------------------//
	btRigidBody* physics::create_box(const vtx::fvtx& org, const vtx::fvtx& size, const vtx::fvtx& dir)
	{
		btCollisionShape* cshape = new btBoxShape(btVector3(size.x, size.y, size.z));
		shapes_.push_back(cshape);
		btRigidBody* body = new_body_(cshape, org, dir);
		body->setDamping(current_.damping_linear_, current_.damping_rotate_);
		world_->addRigidBody(body);
		return body;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	球作成
		@param[in]	org	起点
		@param[in]	radius	半径
		@return RigidBody のポインター
	*/
	//-----------------------------------------------------------------//
	btRigidBody* physics::create_sphere(const vtx::fvtx& org, float radius)
	{
		btCollisionShape* cshape = new btSphereShape(radius);
		shapes_.push_back(cshape);
		btRigidBody* body = new_body_(cshape, org, vtx::fvtx(0.0f, 0.0f, 1.0f));
		body->setDamping(current_.damping_linear_, current_.damping_rotate_);
		world_->addRigidBody(body);
		return body;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	シリンダー作成
		@param[in]	org	起点
		@param[in]	end	終点
		@param[in]	radius	半径
		@return RigidBody のポインター
	*/
	//-----------------------------------------------------------------//
	btRigidBody* physics::create_cylinder(const vtx::fvtx& org, const vtx::fvtx& end, float radius)
	{
		vtx::fvtx d = end - org;
		vtx::fvtx n;
		if(vtx::normalize(d, n)) {
			float len = d.len() * 0.5f;
			btVector3 extent(radius, radius, len);
			btCollisionShape* cshape = new btCylinderShapeZ(extent);
			shapes_.push_back(cshape);
			vtx::fvtx p = (org + end) * 0.5f;	// 中心
			btRigidBody* body = new_body_(cshape, p, n);
			body->setDamping(current_.damping_linear_, current_.damping_rotate_);
			world_->addRigidBody(body);
			return body;
		} else {
			return 0;
		}
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	カプセル作成
		@param[in]	org	起点
		@param[in]	end	終点
		@param[in]	radius	半径
		@param[in]	shave	ジョイント部分を削る場合「true」
		@return btRigidBody のポインター
	*/
	//-----------------------------------------------------------------//
	btRigidBody* physics::create_capsule(const vtx::fvtx& org, const vtx::fvtx& end, float radius)
	{
		vtx::fvtx d = end - org;
		vtx::fvtx n;
		if(vtx::normalize(d, n)) {
			float len = d.len();
			len -= radius * 2;
			btCollisionShape* cshape = new btCapsuleShapeZ(radius, len);
			shapes_.push_back(cshape);
			vtx::fvtx p = (org + end) * 0.5f;	// 中心
			btRigidBody* body = new_body_(cshape, p, n);
			body->setDamping(current_.damping_linear_, current_.damping_rotate_);
			world_->addRigidBody(body);
			return body;
		} else {
			return 0;
		}
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ジョイント作成
		@param[in]	bdya	ボディーＡ
		@param[in]	bdyb	ボディーＢ
		@param[in]	ofsa	ボディーＡオフセット
		@param[in]	ofsb	ボディーＢオフセット
		@return btJoint のポインター
	*/
	//-----------------------------------------------------------------//
	btTypedConstraint* physics::create_joint(btRigidBody* bdya, btRigidBody* bdyb, float ofsa, float ofsb)
	{
		bool useLinearReferenceFrameA = true;
		btTransform trans_a;
		trans_a.setIdentity();
		trans_a.setOrigin(btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(ofsa)));
		btTransform trans_b;
		trans_b.setIdentity();
		trans_b.setOrigin(btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(ofsb)));
		btGeneric6DofConstraint* joint = new btGeneric6DofConstraint(*bdya, *bdyb, trans_a, trans_b,
			useLinearReferenceFrameA);

//		joint->setAngularLowerLimit(btVector3(-SIMD_PI*0.5f,-SIMD_EPSILON,-SIMD_PI*0.5f));
//		joint->setAngularUpperLimit(btVector3( SIMD_PI*0.5f, SIMD_EPSILON, SIMD_PI*0.5f));
		joint->setAngularLowerLimit(btVector3(-SIMD_PI * 0.5f,-SIMD_PI * 0.5f, -SIMD_PI * 0.5f));
		joint->setAngularUpperLimit(btVector3( SIMD_PI * 0.5f, SIMD_PI * 0.5f,  SIMD_PI * 0.5f));

		world_->addConstraint(joint, true);
		joints_.push_back(joint);

		return joint;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	スプリング・ジョイント作成
		@param[in]	bdya	ボディーＡ
		@param[in]	bdyb	ボディーＢ
		@param[in]	ofsa	ボディーＡオフセット
		@param[in]	ofsb	ボディーＢオフセット
		@return btJoint のポインター
	*/
	//-----------------------------------------------------------------//
	btTypedConstraint* physics::create_spring_joint(btRigidBody* bdya, btRigidBody* bdyb, float ofsa, float ofsb)
	{
		bool useLinearReferenceFrameA = true;
		btTransform trans_a;
		trans_a.setIdentity();
		trans_a.setOrigin(btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(ofsa)));
		btTransform trans_b;
		trans_b.setIdentity();
		trans_b.setOrigin(btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(ofsb)));
		btGeneric6DofSpringConstraint* joint = new btGeneric6DofSpringConstraint(*bdya, *bdyb,
			trans_a, trans_b, useLinearReferenceFrameA);

		// 初期位置をバネの復元ポイントとする
		joint->setEquilibriumPoint();

//		joint->enableSpring(0, true);	/// X
//		joint->enableSpring(1, true);	/// Y
//		joint->enableSpring(2, true);	/// Z
		joint->enableSpring(3, true);	/// X
		joint->enableSpring(4, true);	/// Y
		joint->enableSpring(5, true);	/// Z

		// バネの柔らかさ
//		joint->setStiffness(0, 100.0f);	/// X
//		joint->setStiffness(1, 100.0f);	/// Y
//		joint->setStiffness(2, 100.0f);	/// Z
		joint->setStiffness(3, 10.0f);	/// X
		joint->setStiffness(4, 10.0f);	/// Y
		joint->setStiffness(5, 10.0f);	/// Z

		/// 減衰率
		joint->setDamping(0, 0.8f);
		joint->setDamping(1, 0.8f);
		joint->setDamping(2, 0.8f);
		joint->setDamping(3, 0.8f);
		joint->setDamping(4, 0.8f);
		joint->setDamping(5, 0.8f);

		// バネの場合はリミッターを無効にする！
		joint->setAngularLowerLimit(btVector3(1.0f, 1.0f, 1.0f));
		joint->setAngularUpperLimit(btVector3(-1.0f, -1.0f, -1.0f));

		world_->addConstraint(joint, true);
		joints_.push_back(joint);

		return joint;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	剛体生成
		@param[in]	hs	髪の毛構造
	*/
	//-----------------------------------------------------------------//
	void physics::create_body(const hairs& hs)
	{
		BOOST_FOREACH(const hair& h, hs) {
			if(!h.joints_.empty()) {
				vtx::fvtx org = h.joints_[0];
				BOOST_FOREACH(const vtx::fvtx& v, h.joints_) {
					vtx::fvtx d = v - org;
					vtx::fvtx n;
					if(vtx::normalize(d, n)) {
						btVector3 extent(h.radius_, h.radius_, d.len());
						btCollisionShape* cshape = new btCylinderShape(extent);
						shapes_.push_back(cshape);
						vtx::fvtx p = (org + v) * 0.5f;	// 中心
						btRigidBody* body = new_body_(cshape, p, n);
						body->setDamping(0.15f, 0.85f);
						world_->addRigidBody(body);
					}
					org = v;
				}
			}
		}
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	シュミレーション
	*/
	//-----------------------------------------------------------------//
	void physics::update(const gl::camera& cam)
	{
		world_->stepSimulation(1.0f / 60.0f, 10);

		for(int j = world_->getNumCollisionObjects() - 1; j >= 0; --j) {
			btCollisionObject* obj = world_->getCollisionObjectArray()[j];
			btRigidBody* body = btRigidBody::upcast(obj);
			if(body && body->getMotionState()) {
				btTransform trans;
				body->getMotionState()->getWorldTransform(trans);
			}
		}

		if(!cam.get_touch()) {
			using namespace gl;
			IGLcore* igl = get_glcore();
			if(igl == 0) return;
			const device& dev = igl->get_device();

			const vtx::spos& mspos = dev.get_cursor();
			btVector3 ray_to = get_ray_to_(cam, mspos.x, mspos.y);

			if(dev.get_positive(device::key::MOUSE_LEFT)) {
				picking_body_(cam, ray_to);
			}
			if(dev.get_level(device::key::MOUSE_LEFT)) {
				moveing_body_(cam, ray_to);
			} else {
				remove_picking_constraint_();
			}
		}
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	レンダリング
	*/
	//-----------------------------------------------------------------//
	void physics::render()
	{
		if(render_shadow_) {
			glClear(GL_STENCIL_BUFFER_BIT);
			glEnable(GL_CULL_FACE);
			render_world_(0);

			glDisable(GL_LIGHTING);
			glDepthMask(GL_FALSE);
			glDepthFunc(GL_LEQUAL);
			glEnable(GL_STENCIL_TEST);
			glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
			glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
			glFrontFace(GL_CCW);
			glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
			render_world_(1);
			glFrontFace(GL_CW);
			glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
			render_world_(1);
			glFrontFace(GL_CCW);

			glPolygonMode(GL_FRONT,GL_FILL);
			glPolygonMode(GL_BACK,GL_FILL);
			glShadeModel(GL_SMOOTH);
			glEnable(GL_DEPTH_TEST);
			glDepthFunc(GL_LESS);
			glEnable(GL_LIGHTING);
			glDepthMask(GL_TRUE);
			glCullFace(GL_BACK);
			glFrontFace(GL_CCW);
			glEnable(GL_CULL_FACE);
			glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);

			glDepthFunc(GL_LEQUAL);
			glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
			glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
			glDisable(GL_LIGHTING);
			render_world_(2);
			glEnable(GL_LIGHTING);
			glDepthFunc(GL_LESS);
			glDisable(GL_STENCIL_TEST);
			glDisable(GL_CULL_FACE);
		} else {
			glDisable(GL_CULL_FACE);
			render_world_(0);
		}
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	剛体廃棄
	*/
	//-----------------------------------------------------------------//
	void physics::destroy_body()
	{
		for(int i = 0; i < shapes_.size(); ++i) {
			btCollisionShape* shape = shapes_[i];
			delete shape;
		}
		shapes_.clear();
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	廃棄
	*/
	//-----------------------------------------------------------------//
	void physics::destroy()
	{
		delete world_;
		world_ = 0;
		delete solver_;
		solver_ = 0;
		delete broadphase_;
		broadphase_ = 0;
		delete dispatcher_;
		dispatcher_ = 0;
		delete config_;
		config_ = 0;
	}

}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_DIV_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/definition/ceil.hpp>
#include <boost/simd/function/divceil.hpp>
#include <boost/simd/function/definition/floor.hpp>
#include <boost/simd/function/divfloor.hpp>
#include <boost/simd/function/definition/round.hpp>
#include <boost/simd/function/divround.hpp>
#include <boost/simd/function/definition/round2even.hpp>
#include <boost/simd/function/divround2even.hpp>
#include <boost/simd/function/definition/fix.hpp>
#include <boost/simd/function/divfix.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{

  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          , bs::tag::ceil_
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b
                                  , bd::functor<bs::tag::ceil_> const& ) const BOOST_NOEXCEPT
    {
      return divceil(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          , bs::tag::floor_
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b
                                  ,  bd::functor<bs::tag::floor_> const& ) const BOOST_NOEXCEPT
    {
      return divfloor(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          , bs::tag::fix_
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b
                                  ,  bd::functor<bs::tag::fix_> const& ) const BOOST_NOEXCEPT
    {
      return divfix(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          , bs::tag::round_
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b
                                  ,  bd::functor<bs::tag::round_> const& ) const BOOST_NOEXCEPT
    {
      return divround(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          , bs::tag::round2even_
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a, T const& b
                                  ,  bd::functor<bs::tag::round2even_> const& ) const BOOST_NOEXCEPT
    {
      return divround2even(a, b);
    }
  };
} } }

#endif


#pragma once
#include <iostream>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/bimap.hpp>
#include <parsers.hpp>
#include <egraph.hpp>
#include <pagmo/types.hpp>

namespace OptCompiler {

	class Evaluator
	{
		typedef boost::adjacency_list<boost::hash_setS, boost::hash_setS, boost::directedS,
			boost::property<boost::vertex_index_t, std::size_t, boost::property<
			boost::vertex_name_t, int>>,
			boost::no_property> Graph;
		typedef boost::graph_traits<Graph>::vertex_descriptor GVertex;
		typedef boost::graph_traits<Graph>::edge_descriptor GEdge;
		typedef boost::bimap<int, GVertex>::value_type position;
		typedef std::vector<GVertex> container;

		typedef std::map<int, std::shared_ptr<std::vector<int>>> MapIntToUnion;

		struct cycle_detector : public boost::dfs_visitor<>
		{
			cycle_detector(bool& has_cycle)
				: _has_cycle(has_cycle)
			{
			}

			template <class Edge, class Graph>
			void back_edge(Edge, Graph&)
			{
				_has_cycle = true;
			}

		protected:
			bool& _has_cycle;
		};

		class bfs_in_edges_vis : public boost::dfs_visitor<>
		{
		public:
			bfs_in_edges_vis(GVertex& v, std::vector<GVertex>& ie) : targetV(v), inEdges(ie)
			{
			}

			void examine_edge(GEdge e, const Graph& g)
			{
				auto targetVertex = target(e, g);

				if (targetVertex == targetV)
				{
					inEdges.push_back(source(e, g));
				}
			}

			GVertex& targetV;
			std::vector<GVertex>& inEdges;
		};

	public:
		Evaluator();

		virtual ~Evaluator();

		static Evaluator& GetEvaluator(void)
		{
			return *_pcSingleton;
		}

		static void InitEvaluator(void);

		static Evaluator* _pcSingleton;

		// non-static members
		static void ConstructDependencyGraph(
			std::unordered_map<std::string, int>& stockQuery,
			std::vector<std::string> inputMaterials,
			int N,
			Graph& g);

		static void ConstructPartialDependencyGraph(std::unordered_map<std::string, int>& stockQuery,
			std::vector<std::pair<std::string, int>>& inputMaterials,
			Graph& g);

		static double EvaluateCostForSetups(Graph& g,
			Vector2<int>& sharedSetups,
			Vector2<double>& weights,
			std::vector<std::pair<int, int>>& mergedResults,
			std::vector<LLHelm*> references,
			std::unordered_map<int, int>& mergedDict,
			MapIntToUnion& mapIntToUnion,
			bool isStack = false);

		static double EvaluateCostForSetups(Graph& g, std::vector<std::pair<int, int>>& setups);

		static bool ExportProgramsToFile(std::vector<ENode*>& progs, std::ostream& file);

		static std::vector<int> PartialTopologicalSort(const std::vector<int>& elemSet,
			const std::vector<std::string>& inputMaterials,
			int curIndex,
			const boost::property_map<Graph, boost::vertex_name_t>::type&
			depGraphIndex,
			std::unordered_map<std::string, int>& stockQuery);

		static std::vector<int> PartialTopologicalSort(const std::vector<int>& elemSet,
			const boost::property_map<Graph, boost::vertex_name_t>::
			type& depGraphIndex,
			container& oriSortResult);

		static std::vector<double> EvaluatePrograms(std::vector<ENode*>& progs);

		static std::vector<double> EvaluatePrograms(const EncodedVec& vec);

		static void CollapseEdges(GVertex& b, GVertex& a, Graph& g);

		static double GetDimensionPrecision(double num);

		static double GetAnglePrecision(double num);

		static double PrecisionToDouble(int p);
	};
}


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_PHYSICS_HH
#define THERMAL_PHYSICS_HH

#include <Geometry.hh>
#include <HeatSource.hh>
#include <ImplicitOperator.hh>
#include <Physics.hh>
#include <ThermalOperatorBase.hh>

#include <deal.II/base/time_stepping.h>
#include <deal.II/base/time_stepping.templates.h>
#include <deal.II/hp/fe_collection.h>

#include <boost/property_tree/ptree.hpp>

namespace adamantine
{
/**
 * This class takes care of building the linear operator and the
 * right-hand-side. Also used to evolve the system in time.
 */
template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
class ThermalPhysics : public Physics<dim, MemorySpaceType>
{
public:
  /**
   * Constructor.
   * \param[in] database requires the following entries:
   *   - materials: property tree
   *   - sources: property tree
   *   - sources.n_beams: unsigned int in \f$[0,\infty)\f$
   *   - sources.beam_X: property tree with X the number associated to
   *   the heat source
   *   - time_stepping: property tree
   *   - time_stepping.method: string
   *   - time_stepping.coarsening_parameter: double in \f$[0,\infty)\f$
   *   [optional, default value of 1.2]
   *   - time_stepping.refining_parameter: double in \f$(0,1)\f$
   *   [optional, default value of 0.8]
   *   - time_stepping.min_time_step: double in \f$[0,\infty)\f$
   *   [optional, default value of 1e-14]
   *   - time_stepping.max_time_step: double in \f$(0,\infty)\f$
   *   [optional, default value of 1e100]
   *   - time_stepping.refining_tolerance: double in \f$(0,\infty)\f$
   *   [optional, default value of 1e-8]
   *   - time_stepping.coarsening_tolerance: double in \f$(0, \infty)\f$
   *   [optional, default value of 1e-12]
   *   - time_stepping.max_iteration: unsigned int \f$[0,\infty)\f$
   *   [optional, default value of 1000]
   *   - time_stepping.right_preconditioning: boolean [optional, default
   *   value is false]
   *   - time_stepping.n_tmp_vectors: unsigned int in \f$[0,\infty)\f$
   *   [optional, default of 30]
   *   - time_stepping.newton_max_iteration: unsigned int in
   *   \f$[0,\infty)\f$ [optional, default valie of 100]
   *   - time_stepping.newton_tolerance: double in \f$(0,\infty)\f$
   *   [optional, default of 1e-6]
   *   - time_stepping.jfnk: boolean [optional, default value of false]
   */
  ThermalPhysics(MPI_Comm const &communicator,
                 boost::property_tree::ptree const &database,
                 Geometry<dim> &geometry);

  void setup_dofs() override;

  void compute_inverse_mass_matrix() override;

  void add_material(
      std::vector<typename dealii::DoFHandler<dim>::active_cell_iterator> const
          &elements_to_activate,
      double initial_temperature,
      dealii::LA::distributed::Vector<double, MemorySpaceType> &solution)
      override;

  double evolve_one_time_step(
      double t, double delta_t,
      dealii::LA::distributed::Vector<double, MemorySpaceType> &solution,
      std::vector<Timer> &timers) override;

  double get_delta_t_guess() const override;

  void initialize_dof_vector(
      dealii::LA::distributed::Vector<double, MemorySpaceType> &vector)
      const override;

  /**
   * Initialize the given vector. The value is assumed to be a temperature.
   */
  void
  initialize_dof_vector(double const value,
                        dealii::LA::distributed::Vector<double, MemorySpaceType>
                            &vector) const override;

  void get_state_from_material_properties() override;

  void set_state_to_material_properties() override;

  dealii::DoFHandler<dim> &get_dof_handler() override;

  dealii::AffineConstraints<double> &get_affine_constraints() override;

  std::shared_ptr<MaterialProperty<dim>> get_material_property() override;

  /**
   * Return the heat sources.
   */
  std::vector<std::shared_ptr<HeatSource<dim>>> &get_heat_sources();

  /**
   * Return the current height of the heat source.
   */
  double get_current_source_height() const;

private:
  using LA_Vector =
      typename dealii::LA::distributed::Vector<double, MemorySpaceType>;

  /**
   * Compute the right-hand side and apply the TermalOperator.
   */
  LA_Vector evaluate_thermal_physics(double const t, LA_Vector const &y,
                                     std::vector<Timer> &timers) const;

  /**
   * Compute the inverse of the ImplicitOperator.
   */
  LA_Vector id_minus_tau_J_inverse(double const t, double const tau,
                                   LA_Vector const &y,
                                   std::vector<Timer> &timers) const;

  /**
   * This flag is true if the time stepping method is embedded.
   */
  bool _embedded_method = false;
  /**
   * This flag is true if the time stepping method is implicit.
   */
  bool _implicit_method = false;
  /**
   * This flag is true if right preconditioning is used to invert the
   * ImplicitOperator.
   */
  bool _right_preconditioning;
  /**
   * Maximum number of iterations to invert the ImplicitOperator.
   */
  unsigned int _max_iter;
  /**
   * Maximum number of temporary vectors when inverting the ImplicitOperator.
   */
  unsigned int _max_n_tmp_vectors;
  /**
   * Guess of the next time step.
   */
  double _delta_t_guess;
  /**
   * Tolerance to inverte the ImplicitOperator.
   */
  double _tolerance;
  /**
   * Current height of the object.
   */
  double _current_source_height = 0.;
  /**
   * Type of boundary.
   */
  BoundaryType _boundary_type;
  /**
   * Associated geometry.
   */
  Geometry<dim> &_geometry;
  /**
   * Associated Lagrange finite elements.
   */
  dealii::hp::FECollection<dim> _fe_collection;
  /**
   * Associated DoFHandler.
   */
  dealii::DoFHandler<dim> _dof_handler;
  /**
   * Associated AffineConstraints<double>.
   */
  dealii::AffineConstraints<double> _affine_constraints;
  /**
   * Associated quadature, either Gauss or Gauss-Lobatto.
   */
  dealii::hp::QCollection<1> _q_collection;
  /**
   * Shared pointer to the material properties associated to the domain.
   */
  std::shared_ptr<MaterialProperty<dim>> _material_properties;
  /**
   * Vector of heat sources.
   */
  std::vector<std::shared_ptr<HeatSource<dim>>> _heat_sources;
  /**
   * Shared pointer to the underlying ThermalOperator.
   */
  std::shared_ptr<ThermalOperatorBase<dim, MemorySpaceType>> _thermal_operator;
  /**
   * Unique pointer to the underlying ImplicitOperator.
   */
  std::unique_ptr<ImplicitOperator<MemorySpaceType>> _implicit_operator;
  /**
   * Shared pointer to the underlying time stepping scheme.
   */
  std::unique_ptr<dealii::TimeStepping::RungeKutta<LA_Vector>> _time_stepping;
};

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline double ThermalPhysics<dim, fe_degree, MemorySpaceType,
                             QuadratureType>::get_delta_t_guess() const
{
  return _delta_t_guess;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline dealii::DoFHandler<dim> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_dof_handler()
{
  return _dof_handler;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline dealii::AffineConstraints<double> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_affine_constraints()
{
  return _affine_constraints;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline std::shared_ptr<MaterialProperty<dim>>
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_material_property()
{
  return _material_properties;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline std::vector<std::shared_ptr<HeatSource<dim>>> &
ThermalPhysics<dim, fe_degree, MemorySpaceType,
               QuadratureType>::get_heat_sources()
{
  return _heat_sources;
}

template <int dim, int fe_degree, typename MemorySpaceType,
          typename QuadratureType>
inline double ThermalPhysics<dim, fe_degree, MemorySpaceType,
                             QuadratureType>::get_current_source_height() const
{
  return _current_source_height;
}
} // namespace adamantine

#endif


#include "Tracking/Tracker.h"
#include "Tracking/LinearAssignment.h"
#include <algorithm>
#include <numeric>
#include <iterator>
#include <functional>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <limits>
//#include "XML/XMLReader.h"
//#include "XML/XMLBase.h"
#include "Utils/Exception.h"
#include "Utils/CVROI.h"
#include <boost/bind.hpp>
#include "Serialisation/json.h"

using namespace std;
using namespace cv;
using namespace Tracking::LA;
//using namespace XML;
using namespace Utils;

namespace {
	template<typename T>
    Point_<T> BoxCentre (const cv::Rect_<T>& box)
    {
        return Point_<T>((box.br().x+box.tl().x)/2, (box.br().y+box.tl().y)/2);
    }

	template<typename T>
	bool IsBoxesClosed(cv::Rect_<T> box1, cv::Rect_<T> box2, const cv::Size& extension = cv::Size(5, 5))
	{
		// find the bigger box, and always set box1 as the bigger one
		if (box1.width*box1.height < box2.width*box2.height)
			std::swap(box1, box2);

		int boxWidth = box2.width/2;
		int boxHeight = box2.height/2;
		boxWidth += extension.width;
		boxHeight += extension.height;
		box1.width += 2*boxWidth;
		box1.height += 2*boxHeight;
		box1.x -= boxWidth;
		box1.y -= boxHeight;

		return box1.contains(cv::Point(BoxCentre(box2)));
	}

	Rect MergeBoxesImpl(const cv::Rect& box1, const cv::Rect& box2)
	{
		int tlx = std::min(box1.tl().x, box2.tl().x);
		int tly = std::min(box1.tl().y, box2.tl().y);
		int brx = std::max(box1.br().x, box2.br().x);
		int bry = std::max(box1.br().y, box2.br().y);

		return Rect(tlx, tly, brx - tlx + 1, bry - tly + 1);
	}

	std::vector<cv::Rect> MergeBoxes(const std::vector<cv::Rect>& boxes, const cv::Size& extension = cv::Size(5, 5))
	{
		int numBoxes = boxes.size();
		if (numBoxes == 0)
			return vector<Rect>();

		std::unique_ptr<int[]> mergedIndex(new int[numBoxes]());
		vector<Rect> mergedBoxes(boxes.begin(), boxes.end());

		for (int i = 0; i < numBoxes - 1; ++i)
		{
			for (int j = i + 1; j < numBoxes; ++j)
			{
				if (mergedIndex[i] == 0 && mergedIndex[j] == 0 && IsBoxesClosed(boxes[i], boxes[j], extension))
				{
					mergedBoxes[i] = MergeBoxesImpl(mergedBoxes[i], mergedBoxes[j]);
					mergedIndex[j] = 1;
				}
			}
		}

		// remove redundancy
		int index = 0;
		mergedBoxes.erase(std::remove_if(mergedBoxes.begin(), mergedBoxes.end(), [&mergedIndex, index](const cv::Rect& box) mutable { return mergedIndex[index++]; }), mergedBoxes.end());

		return mergedBoxes;
	}

	enum class EAxis { X, Y };
	bool CoordinateInFrame(const cv::Size& frameSize, int coordinate, EAxis axis)
	{
		switch (axis)
		{
		case EAxis::X:
			return coordinate >= 0 && coordinate < frameSize.width ? true : false;
		case EAxis::Y:
			return coordinate >= 0 && coordinate < frameSize.height ? true : false;
		default:
			return false;
		}
	}

	int CalcHistogramIndex(const cv::Vec3b& colour, int numBins)
	{
		int index = 0;
		int I[3] = {0};
		int binValue = 256 / numBins;
		for (int k = 0; k < 3; ++k)
		{
			I[k] = static_cast<int>(std::floor(colour.val[k] / binValue));
			if (k < 2)
				index += I[k] * static_cast<int>(std::pow(numBins, 2 - k));
			else
				index += I[k];
		}

		return index;
	}

	struct SCostCalc
    {
        SCostCalc(const cv::Rect_<float>& box, float alpha)
			: m_centre(BoxCentre(box))
			, m_height(box.height)
			, m_width(box.width)
			, m_alpha(alpha)
		{
		}

        template <typename T>
        float operator()(const cv::Rect_<T>& t) const
        {
			auto tCentre = BoxCentre(t);
			return m_alpha * std::pow((m_centre.x-tCentre.x), 2) + std::pow((m_centre.y-tCentre.y), 2) +
				(1-m_alpha) * std::abs((m_width-t.width)*(m_height-t.height));
        }

    private:
		cv::Point_<float> m_centre;
		float m_height;
		float m_width;
        float m_alpha;
    };

}

namespace Tracking {
    namespace Tracker {

		bool IsBoxCloseToEntries(const cv::Rect& box, const cv::Size frameSize, const std::vector<cv::Rect>* definedEntries)
		{
			// check if quater box is close to the frame boundaries
			Rect qbox(box.x + box.width / 4, box.y + box.height, box.width / 2, box.height / 2);
			if (!CoordinateInFrame(frameSize, qbox.tl().x, EAxis::X) || !CoordinateInFrame(frameSize, qbox.br().x, EAxis::X) ||
				!CoordinateInFrame(frameSize, qbox.tl().y, EAxis::Y) || !CoordinateInFrame(frameSize, qbox.br().y, EAxis::X))
				return true;

			// actually check if box centre close to entries
			cv::Point centre = BoxCentre(box);
			// check if close to any predefined exit points
			if (definedEntries != nullptr && !definedEntries->empty())
			{
				for (const auto& entry: *definedEntries)
				{
					if (entry.contains(centre))
						return true;
				}
			}

			return false;
		}

		bool IsBoxesCloseToEntries(const std::vector<cv::Rect>& boxes, const cv::Size frameSize, const std::vector<cv::Rect>* definedEntries)
		{
			bool retval = true;
			for (auto& box: boxes)
			{
				retval = retval && IsBoxCloseToEntries(box, frameSize, definedEntries);
			}

			return retval;
		}
		/****************************************/
		// Kalman tracker
		/****************************************/
		SKalmanParam::SKalmanParam(const std::string& configFile)
		{
			CREQUIRE(!configFile.empty(), "No configuration file specified");

			Json::Reader reader;
			Json::Value root;
			std::ifstream ifs(configFile.c_str());
			CREQUIRE(ifs.is_open(), "Error opening configuration file: " + configFile);
			CREQUIRE(reader.parse(ifs, root), "Error parsing configuration file: " + reader.getFormattedErrorMessages());

			const auto& config = root["Tracking"]["KalmanFilter"];

			m_maxOverlapPropagate = config["MaxOverlapPropagation"].asInt();
			m_maxLostPropagate = config["MaxLostPropagation"].asInt();
			m_maxPropagateNoRecord = config["MaxPropagationNoRecord"].asInt();
		}

		std::unique_ptr<const SKalmanParam> CKalmanFilter::m_kalmanParam(nullptr);
		void CKalmanFilter::SetGlobalConfig(const std::string& configFile)
		{
			m_kalmanParam.reset(new SKalmanParam(configFile));
		}

		void CKalmanFilter::Init(const cv::Rect& box)
		{
			if (!m_kalmanParam)
				throw CException("Global configuration parameters are not set");

			m_tracker.init(STATE_D, MEASURE_D);
			// set transition matrix
			setIdentity(m_tracker.transitionMatrix, Scalar_<float>(1.0f));
			m_tracker.transitionMatrix.at<float>(0,4) = 1.0f;
			m_tracker.transitionMatrix.at<float>(1,5) = 1.0f;
			m_tracker.transitionMatrix.at<float>(2,6) = 1.0f;
			m_tracker.transitionMatrix.at<float>(3,7) = 1.0f;
            // set measurement matrix
			setIdentity(m_tracker.measurementMatrix, Scalar_<float>(1.0f));
			// set noise cov
            setIdentity(m_tracker.processNoiseCov, Scalar_<float>::all(1e-4f));
            setIdentity(m_tracker.measurementNoiseCov, Scalar_<float>::all (1e-1f));
            setIdentity(m_tracker.errorCovPre, Scalar_<float>::all(1.0f));
            setIdentity(m_tracker.errorCovPost, Scalar_<float>::all(1.0f));
			// set initial state
            m_tracker.statePre.at<float>(0) = static_cast<float>(box.x);
            m_tracker.statePre.at<float>(1) = static_cast<float>(box.y);
            m_tracker.statePre.at<float>(2) = static_cast<float>(box.width);
            m_tracker.statePre.at<float>(3) = static_cast<float>(box.height);
            m_tracker.statePre.copyTo(m_tracker.statePost);

			Mat kalmanMeasure = m_tracker.measurementMatrix * m_tracker.predict();
			m_kalmanMeasure.x = static_cast<int>(kalmanMeasure.at<float>(0));
			m_kalmanMeasure.y = static_cast<int>(kalmanMeasure.at<float>(1));
			m_kalmanMeasure.width = static_cast<int>(kalmanMeasure.at<float>(2));
			m_kalmanMeasure.height = static_cast<int>(kalmanMeasure.at<float>(3));

			m_hasInit = true;
			m_hasMeasureUpdated = false;
		}

		void CKalmanFilter::Update(const cv::Rect& box)
		{
			if (!m_hasInit)
				return;

			Mat measure = Mat(MEASURE_D, 1, CV_32FC1);
			float* mptr = measure.ptr<float>();
			mptr[0] = static_cast<float>(box.x);
			mptr[1] = static_cast<float>(box.y);
			mptr[2] = static_cast<float>(box.width);
			mptr[3] = static_cast<float>(box.height);
			m_tracker.correct(measure);

			m_hasMeasureUpdated = false;
		}

		const cv::Rect& CKalmanFilter::Measure() const
		{
			if (m_hasMeasureUpdated)	// if !m_hasInit, the return will be a default initialised Mat
				return m_kalmanMeasure;

			Mat kalmanMeasure = m_tracker.measurementMatrix * m_tracker.predict();
			m_kalmanMeasure.x = static_cast<int>(kalmanMeasure.at<float>(0));
			m_kalmanMeasure.y = static_cast<int>(kalmanMeasure.at<float>(1));
			m_kalmanMeasure.width = std::max(static_cast<int>(kalmanMeasure.at<float>(2)), 0);
			m_kalmanMeasure.height = std::max(static_cast<int>(kalmanMeasure.at<float>(3)), 0);

			m_hasMeasureUpdated = true;
			return m_kalmanMeasure;
		}

		const cv::Mat& CKalmanFilter::StatePost() const
		{
			return m_tracker.statePost;
		}

		bool CKalmanFilter::HasMeasureUpdated() const
		{
			return m_hasMeasureUpdated;
		}

		void CKalmanFilter::Release()
		{
			m_tracker = KalmanFilter();
			m_kalmanMeasure = Rect();
			m_hasMeasureUpdated = false;
			m_hasInit = false;
		}

		/****************************************/
		// MeanShift tracker
		/****************************************/
		SMeanShiftParam::SMeanShiftParam(const std::string& configFile)
		{
			CREQUIRE(!configFile.empty(), "No configuration file specified");

			Json::Reader reader;
			Json::Value root;
			std::ifstream ifs(configFile.c_str());
			CREQUIRE(ifs.is_open(), "Error opening configuration file: " + configFile);
			CREQUIRE(reader.parse(ifs, root), "Error parsing configuration file: " + reader.getFormattedErrorMessages());

			const auto& config = root["Tracking"]["MeanShiftFilter"];

			m_numBins = config["NumberBins"].asInt();
			m_maxIter = config["MaxIteration"].asInt();
			m_histUpdateRate = config["HistogramUpdateRate"].asFloat();
			m_BHCoeffThreshold = config["BHCoeffThreshold"].asFloat();
		}

		std::unique_ptr<const SMeanShiftParam> CMeanShiftFilter::m_meanshiftParam(nullptr);
		void CMeanShiftFilter::SetGlobalConfig(const std::string& configFile)
		{
			m_meanshiftParam.reset(new SMeanShiftParam(configFile));
		}

		void CMeanShiftFilter::Init(const cv::Rect& box, const cv::Mat& frame, const cv::Mat* fg)
		{
			if (!m_meanshiftParam)
				throw CException("Global configuration parameters are not set");

			Mat kernel;
			CalcEpanechnikovKernel(box, &kernel);
			// calculate colour histogram
			CalcHistogram(box, kernel, frame, fg, m_meanshiftParam->m_numBins, &m_histogram, &m_histogramVol);
			m_meanshiftMeasure = box;
			m_BHCoeff = 1.0f;
			m_hasInit = true;
			m_hasMeasureUpdated = false;
		}

		void CMeanShiftFilter::CalcHistogram(const cv::Rect& box, const cv::Mat& kernel, const cv::Mat& frame, const cv::Mat* fg, int numBins, cv::Mat* hist, float* histVol)
		{
			// calculate colour histogram
			int totalNumBins = static_cast<int>(std::pow(numBins, frame.channels()));
			hist->create(1, totalNumBins, CV_32FC1);
			hist->setTo(0);
			*hist = *hist + (1.0f/totalNumBins);
			*histVol = 1;

			for (int y = 0; y < box.height; ++y)
			{
				int coordinateY = box.tl().y + y;
				if (!CoordinateInFrame(frame.size(), coordinateY, EAxis::Y))
					continue;

				for (int x = 0; x < box.width; ++x)
				{
					int coordinateX = box.tl().x + x;
					if (!CoordinateInFrame(frame.size(), coordinateX, EAxis::X))
						continue;

					Vec3b colour = frame.at<Vec3b>(coordinateY, coordinateX);
					int index = CalcHistogramIndex(colour, numBins);
					float kernelValue = kernel.at<float>(y, x);

					if (fg)
					{
						hist->at<float>(index) += kernelValue * (fg->at<uchar>(coordinateY, coordinateX) + 1);
						*histVol += kernelValue * (fg->at<uchar>(coordinateY, coordinateX) + 1);
					}
					else
					{
						hist->at<float>(index) += kernelValue;
						*histVol += kernelValue;
					}

				}
			}

		}

		float CMeanShiftFilter::BHCoeffThreshold()
		{
			if (m_meanshiftParam.get())
				return m_meanshiftParam->m_BHCoeffThreshold;
			else
				return 0.8;
		}

		void CMeanShiftFilter::CalcEpanechnikovKernel(const cv::Rect& box, cv::Mat* kernel)
		{
			kernel->create(box.height, box.width, CV_32FC1);
			kernel->setTo(0);
			Point centre = Point((box.height + 1) / 2, (box.width + 1) / 2);
			float denominator = (box.width * box.width + box.height * box.height) * 0.25f;
			for (int x = 0; x < box.height; ++x)
			{
				for (int y = 0; y < box.width; ++y)
				{
					float r2 = ((x - centre.x) * (x - centre.x) + (y - centre.y) * (y - centre.y)) / denominator;
					if (r2 < 1)
						kernel->at<float>(x,y) = 1 - r2;
				}
			}
		}

		void CMeanShiftFilter::CalcWeight(const cv::Rect& box, const cv::Mat& kernel, const cv::Mat& frame, const cv::Mat* fg, int numBins, const cv::Mat& hist, float histVol, cv::Mat* weight)
		{
			Mat candidateHist = hist;
			float candidateHistVol = histVol;
			weight->create(box.height, box.size().width, CV_32FC1);
			weight->setTo(0);
			float TH = 0.0f;
			float CH = 0.0f;
			for (int y = 0; y < box.height; ++y)
			{
				int coordinateY = box.tl().y + y;
				if (!CoordinateInFrame(frame.size(), coordinateY, EAxis::Y))
					continue;

				for (int x = 0; x < box.width; ++x)
				{
					int coordinateX = box.tl().x + x;
					if (!CoordinateInFrame(frame.size(), coordinateX, EAxis::X))
						continue;

					Vec3b colour = frame.at<Vec3b>(coordinateY, coordinateX);
					int index = CalcHistogramIndex(colour, numBins);

					if (m_histogramVol > std::numeric_limits<float>::epsilon())
						TH = m_histogram.at<float>(index)/m_histogramVol;
					if (candidateHistVol > std::numeric_limits<float>::epsilon())
						CH = candidateHist.at<float>(index)/candidateHistVol;

					if (CH > std::numeric_limits<float>::epsilon())
					{
						float tmpWeight = std::sqrt(TH/CH);
						if (fg && fg->at<uchar>(coordinateY, coordinateX))
							tmpWeight += 2;

						weight->at<float>(y, x) = std::min<float>(tmpWeight, 1e5);
					}

				}
			}
		}

		float CMeanShiftFilter::CalcBhattacharyya(const cv::Mat& targetHist, float targetHistVol, const cv::Mat& candidateHist, float candidateHistVol)
		{
			Mat tmp;
			cv::sqrt(targetHist.mul(candidateHist), tmp);
			return static_cast<float>(cv::sum(tmp)[0]) / std::sqrt(targetHistVol * candidateHistVol);
		}

		void CMeanShiftFilter::Update(const cv::Rect& box, const cv::Mat& frame, const cv::Mat* fg)
		{
			if (!m_hasInit)
				return;

			// prepare candidate information, predicted track's centre is used for MS
			Rect candidateBox = box;
			Point candidateCentre = BoxCentre(candidateBox);
			Mat candidateKernel;
			CalcEpanechnikovKernel(candidateBox, &candidateKernel);

			Mat candidateHist;
			float candidateHistVol = 0.0f;
			for (int iterCount = 0; iterCount < m_meanshiftParam->m_maxIter; ++iterCount)
			{
				float dx = 0.0f;
				float dy = 0.0f;
				float sumt = 0.0f;

				// meanshift update
				CalcHistogram(candidateBox, candidateKernel, frame, fg, m_meanshiftParam->m_numBins, &candidateHist, &candidateHistVol);
				Mat weight;
				CalcWeight(candidateBox, candidateKernel, frame, fg, m_meanshiftParam->m_numBins, candidateHist, candidateHistVol, &weight);

				for (int y = 0; y < candidateBox.height; ++y)
				{
					for (int x = 0; x < candidateBox.width; ++x)
					{
						float kw = candidateKernel.at<float>(y, x) * weight.at<float>(y, x);
						sumt += kw;
						dx += kw * (x - candidateBox.width/2);
						dy += kw * (y - candidateBox.height/2);
					}
				}

				if (sumt > std::numeric_limits<float>::epsilon())
				{
					dx /= sumt;
					dy /= sumt;
				}

				candidateBox.x += static_cast<int>(dx);
				candidateBox.y += static_cast<int>(dy);

				if (std::abs(dx) < 0.5 && std::abs(dy) < 0.5)
					break;
			}

			m_meanshiftMeasure = candidateBox;
			// calculate BhCoef for measuring tracking performance
			m_BHCoeff = CalcBhattacharyya(m_histogram, m_histogramVol, candidateHist, candidateHistVol);

			// update histogram information
			float vol = (candidateHistVol + m_histogramVol) * 0.5f;
			float currW = vol * m_meanshiftParam->m_histUpdateRate / candidateHistVol;
			float prevW = vol * (1 - m_meanshiftParam->m_histUpdateRate) / m_histogramVol;
			m_histogram = m_histogram * prevW + candidateHist * currW;
			m_histogramVol = static_cast<float>(cv::sum(m_histogram)[0]);

			m_hasMeasureUpdated = true;
		}

		const cv::Rect& CMeanShiftFilter::Measure() const
		{
			return m_meanshiftMeasure;
		}

		double CMeanShiftFilter::ConfidenceLevel() const
		{
			return m_BHCoeff;
		}

		bool CMeanShiftFilter::HasMeasureUpdated() const
		{
			return m_hasMeasureUpdated;
		}

		void CMeanShiftFilter::SetMeasureUpdated(bool value)
		{
			m_hasMeasureUpdated = false;
		}

		void CMeanShiftFilter::Release()
		{
			m_histogram.release();
			m_histogramVol = 0.0f;
			m_meanshiftMeasure = Rect();
			m_BHCoeff = 0.0f;
			m_hasMeasureUpdated = false;
			m_hasInit = false;
		}

		/****************************************/
		// tracker information
		/****************************************/
		void STrackerInfo::Init(const cv::Mat& frame, const cv::Mat* fg, int id, long startFrame, const cv::Rect& box, EObjectCategory objectType, int reservedTrajectoryLength)
        {
			InitKalmanOnly(id, startFrame, box, objectType, reservedTrajectoryLength);
			m_meanshift.Init(box, frame, fg);
        }

		void STrackerInfo::InitKalmanOnly(int id, long startFrame, const cv::Rect& box, EObjectCategory objectType, int reservedTrajectoryLength)
		{
            m_id                      = id;
            m_startFrame              = startFrame;
            m_duration                = 0;
            m_propagation             = 0;
			m_totalPropagation        = 0;
            m_validityScore           = 0.0f;
            m_type                    = objectType;
			m_status.AddStatus(SObjectStatus::Entering);
			m_kalman.Init(box);
			m_meanshift.Release();
			m_features.m_boundingBoxes.reserve(reservedTrajectoryLength);
			m_features.m_boundingBoxes.push_back(box);
			m_features.m_latestBoundingBox = box;
		}

		void STrackerInfo::Release()
		{
			m_id                 = 0;
			m_startFrame         = 0;
			m_duration           = 0;
			m_propagation        = 0;
			m_totalPropagation   = 0;
			m_validityScore      = 0.0f;
			m_type          = EObjectCategory::Unknown;
			m_kalman.Release();
			m_meanshift.Release();
			m_features.Release();
		}

		/****************************************/
		// data association parameters
		/****************************************/
		SDataAssociationParam::SDataAssociationParam(const std::string& configFile)
		{
			CREQUIRE(!configFile.empty(), "No configuration file specified");

			Json::Reader reader;
			Json::Value root;
			std::ifstream ifs(configFile.c_str());
			CREQUIRE(ifs.is_open(), "Error opening configuration file: " + configFile);
			CREQUIRE(reader.parse(ifs, root), "Error parsing configuration file: " + reader.getFormattedErrorMessages());

			const auto& config = root["Tracking"]["DataAssociation"];

			m_costAlpha = config["Alpha"].asFloat();
			m_costDistMax = config["MaxDistance"].asFloat();
			m_costType = config["CostType"].asString();
			m_algorithmType = config["AlgorithmType"].asString();
		}

		/****************************************/
		// miscellaneous parameters
		/****************************************/
		STrackerMiscellaneousness::STrackerMiscellaneousness(float objSizeUpdateRate, int minDispDuration, int stopDuration, int maxAllowedTracks, const cv::Size& minBox, const cv::Size& extension)
			: m_objSizeUpdateRate(objSizeUpdateRate)
			, m_minDispDuration(minDispDuration)
			, m_stopDuration(stopDuration)
			, m_maxAllowedTracks(maxAllowedTracks)
			, m_minBox(minBox)
			, m_extension(extension)
		{
		}

		STrackerMiscellaneousness::STrackerMiscellaneousness(const std::string& configFile)
		{
			CREQUIRE(!configFile.empty(), "No configuration file specified");

			Json::Reader reader;
			Json::Value root;
			std::ifstream ifs(configFile.c_str());
			CREQUIRE(ifs.is_open(), "Error opening configuration file: " + configFile);
			CREQUIRE(reader.parse(ifs, root), "Error parsing configuration file: " + reader.getFormattedErrorMessages());

			const auto& config = root["Tracking"]["Miscellaneousness"];
			m_objSizeUpdateRate = config["ObjectSizeUpdateRate"].asFloat();
			m_minDispDuration = config["MinDisplayDuration"].asInt();
			m_stopDuration = config["StopDuration"].asInt();
			m_maxAllowedTracks = config["MaxAllowedTracks"].asInt();
			m_minBox.height = config["MinBoxHeight"].asInt();
			m_minBox.width = config["MinBoxWidth"].asInt();
			m_extension.height = config["ExtensionHight"].asInt();
			m_extension.width = config["ExtensionWidth"].asInt();
		}

		/****************************************/
		// multiple object tracker
		/****************************************/
		CMultiTracker::CMultiTracker(const SDataAssociationParam& assocParam, const STrackerMiscellaneousness& miscellaneousness, const cv::Size& frameSize)
			: m_frameCount()
			, m_totalTrackCount()
			, m_totalRecordedTrackCount()
			, m_tracks()
			, m_dataAssocParam(assocParam)
			, m_miscellaneousness(miscellaneousness)
			, m_objectPool()
		{
		}

		CMultiTracker::CMultiTracker(const std::string& configFile)
			: m_frameCount()
			, m_totalTrackCount()
			, m_totalRecordedTrackCount()
			, m_tracks()
			, m_dataAssocParam(configFile)
			, m_miscellaneousness(configFile)
			, m_objectPool()
		{
			CKalmanFilter::SetGlobalConfig(configFile);
			CMeanShiftFilter::SetGlobalConfig(configFile);
		}

		void CMultiTracker::InitTrack(const cv::Rect& box)
		{
			STrackerInfo& newTrack = m_objectPool.AcquireObject();
			newTrack.InitKalmanOnly(m_totalTrackCount++, m_frameCount, box, EObjectCategory::Unknown, PRESERVED_TRAJECTORY_LENGTH);
			m_tracks.push_back(&newTrack);
		}

		void CMultiTracker::ReleaseTrack(STrackerInfo& obj)
		{
			obj.Release();
			m_objectPool.ReleaseObject(obj);
		}

		bool CMultiTracker::IsInvalidBox(const cv::Rect& box) const
		{
			return box.width < m_miscellaneousness.m_minBox.width || box.height < m_miscellaneousness.m_minBox.height ? true : false;
		}

		void CMultiTracker::CalcCostMatrix(const std::vector<cv::Rect>& boxes, std::vector<std::vector<float> >* costMatrix, std::vector<std::vector<int> >* gate) const
		{
			auto costDist = m_dataAssocParam.m_costDistMax;
			for (size_t i = 0; i < costMatrix->size(); ++i)
			{
				std::transform(boxes.begin(), boxes.end(), (*costMatrix)[i].begin(), SCostCalc(m_tracks[i]->m_kalman.Measure(), m_dataAssocParam.m_costAlpha));
				if (gate != nullptr)
					std::transform((*costMatrix)[i].begin(), (*costMatrix)[i].end(), (*gate)[i].begin(), [costDist](float value){ return value < costDist; });
			}
		}

		int CMultiTracker::FindBestMatchedObject(const cv::Rect& observation, const std::vector<int>& associationIndex) const
		{
			vector<int> objIndex;
			for (size_t i = 0; i < associationIndex.size(); ++i)
			{
				if (associationIndex[i] == 1)
					objIndex.push_back(i);
			}

			vector<float> scores;
			for (size_t i = 0; i < objIndex.size(); ++i)
			{
				float objBoxArea = static_cast<float>(m_tracks[objIndex[i]]->m_kalman.Measure().width * m_tracks[objIndex[i]]->m_kalman.Measure().height);
				float obsBoxArea = static_cast<float>(observation.width * observation.height);
				scores.push_back(abs(objBoxArea-obsBoxArea)/obsBoxArea);
			}

			vector<float>::iterator minIter = std::min_element(scores.begin(), scores.end());
			if (minIter != scores.end())
				return objIndex[std::distance(scores.begin(), minIter)];

			return -1;
		}

		void CMultiTracker::MergeSplittedObject(std::vector<cv::Rect>* observations, std::vector<std::vector<int> >* assocMatrix, const cv::Size& extension) const
		{
			int numObservations = observations->size();
			int numTracks = assocMatrix->size();

			vector<int> assocObsInd(numObservations, 0);
			for (int i = 0; i < numTracks; ++i)
			{
				for (int j = 0; j < numObservations; ++j)
					assocObsInd[j] += (*assocMatrix)[i][j];
			}

			vector<int> unassocObsInd;
			for (size_t i = 0; i < assocObsInd.size(); ++i)
			{
				if (assocObsInd[i] == 0)
					unassocObsInd.push_back(i);
			}

			vector<int> mergedObsInd(numObservations, 0);
			vector<int> reassocInd(numTracks, 0);
			for (size_t i = 0; i < unassocObsInd.size(); ++i)
			{
				for (int j = 0; j < numTracks; ++j)
				{
					const auto& unassocObsBox = (*observations)[unassocObsInd[i]];
					const auto& predictedTrackBox = m_tracks[j]->m_kalman.Measure();
					if (IsBoxesClosed(predictedTrackBox, unassocObsBox, extension))
					{
						auto index = std::distance((*assocMatrix)[j].begin(), std::find((*assocMatrix)[j].begin(), (*assocMatrix)[j].end(), 1));
						if (index == numObservations) // in case of track has no association
							reassocInd[j] = 1;
						else
						{
							const auto& obsBox = (*observations)[index];
							auto obsCentre = BoxCentre(obsBox);
							auto unassocObsCentre = BoxCentre(unassocObsBox);
							float dist = std::pow(static_cast<float>(obsCentre.x - unassocObsCentre.x), 2) + std::pow(static_cast<float>(obsCentre.y - unassocObsCentre.y), 2);
							float boxThreshold = std::pow(obsBox.width + unassocObsBox.width * 0.5f + extension.width, 2) +
												 std::pow(obsBox.height + unassocObsBox.height * 0.5f + extension.height, 2);
							if (dist < std::max(m_dataAssocParam.m_costDistMax, boxThreshold))
							{
								mergedObsInd[unassocObsInd[i]] = 1;
								(*observations)[index] = MergeBoxesImpl((*observations)[index], (*observations)[unassocObsInd[i]]);
							}
						}
					}
				}

				if (std::any_of(reassocInd.begin(), reassocInd.end(), [](int value) { return value == 1; }))
				{
					// find the best match for the track without observation
					int index = FindBestMatchedObject((*observations)[unassocObsInd[i]], reassocInd);
					if (index >= 0)
						(*assocMatrix)[index][unassocObsInd[i]] = 1;	// add new association in the matrix
				}
			}

			// delete merged observations in the observation list
			int count = 0;
			observations->erase(std::remove_if(observations->begin(), observations->end(), [&mergedObsInd, &count](const Rect& observation) mutable { return mergedObsInd[count++] == 1; }), observations->end());

			vector<vector<int> > refinedAssocMatrix;
			for (int i = 0; i < numTracks; ++i)
			{
				vector<int> rowAssoc;
				int count = 0;
				std::copy_if((*assocMatrix)[i].begin(), (*assocMatrix)[i].end(), std::back_inserter(rowAssoc), [&mergedObsInd, count](int elem) mutable { return mergedObsInd[count++] == 0; });
				refinedAssocMatrix.push_back(std::move(rowAssoc));
			}

			refinedAssocMatrix.swap(*assocMatrix);
		}

		void CMultiTracker::CalcStatusMatrix(const std::vector<cv::Rect>& observations, std::vector<std::vector<int> >* assocMatrix, std::vector<std::vector<int> >* statusMatrix, const cv::Size& extension) const
		{
			vector<int> assocTracksInd(assocMatrix->size(), 0);
			std::transform(assocMatrix->begin(), assocMatrix->end(), assocTracksInd.begin(), [](const vector<int>& rowAssoc){ return std::find(rowAssoc.begin(), rowAssoc.end(), 1) != rowAssoc.end(); });

			vector<int> assocObsInd(observations.size(), 0);
			for (size_t i = 0; i < assocMatrix->size(); ++i)
			{
				for (size_t j = 0; j < observations.size(); ++j)
				{
					assocObsInd[j] += (*assocMatrix)[i][j];
				}
			}

			// originally, we only use predicted objects box position and observation FG
			// to decide if two tracks are overlapped, this is not sufficient enough.
			// Because the remaining shadow after shadow removing in the FG detection
			// part may occasionally join two closed tracks (which outside the range of
			// extension), then in the loop, we can not confirm the two tracks are
			// closed, further more, no overlapping. In order to improve the detection,
			// we should also consider the observation FG. If the FG overlapped with both
			// tracks, even if the two tracks are not closed, they are still overlapped.
			for (size_t i = 0; i < assocMatrix->size(); ++i)
			{
				if (!m_tracks[i]->m_status.HasStatus(SObjectStatus::Entering))	// for unstable detection, we don't count its overlapping case
				{
					for (size_t j = 0; j < assocMatrix->size(); ++j)
					{
						if (i != j && !m_tracks[j]->m_status.HasStatus(SObjectStatus::Entering))
						{
							bool isClosed = IsBoxesClosed(m_tracks[i]->m_kalman.Measure(), m_tracks[j]->m_kalman.Measure(), extension);
							if (isClosed && assocTracksInd[i] && assocTracksInd[j])
								(*statusMatrix)[i][j] = IS_CLOSED;
							else if (isClosed && (assocTracksInd[i] || assocTracksInd[j]) && !(assocTracksInd[i] && assocTracksInd[j]))
								(*statusMatrix)[i][j] = IS_OVERLAPPED;
						}
					}
				}
			}

			for (size_t i = 0; i < assocMatrix->size(); ++i)
			{
				if (!m_tracks[i]->m_status.HasStatus(SObjectStatus::Entering))
				{
					for (size_t j = 0; j < observations.size(); ++j)
					{
						if ((*assocMatrix)[i][j] == 0)
						{
							if (IsBoxesClosed(m_tracks[i]->m_kalman.Measure(), observations[j], Size(0, 0)))
							{
								if (assocObsInd[j])
								{
									// track should has association, find associated track
									int trackInd = -1;
									for (size_t k = 0; k < assocMatrix->size(); ++k)
									{
										if ((*assocMatrix)[k][j] == 1)
										{
											trackInd = k;
											break;
										}
									}

									// mark the tracks as overlapped
									if (trackInd >= 0 && (*assocMatrix)[trackInd][j] == 0)
									{
										(*statusMatrix)[i][trackInd] = IS_OVERLAPPED;
										(*statusMatrix)[trackInd][i] = IS_OVERLAPPED;
									}
								}
								else
								{
									if (assocTracksInd[i] == 0)
									{
										(*statusMatrix)[i][j] = IS_OVERLAPPED;
										(*statusMatrix)[j][i] = IS_OVERLAPPED;
									}
								}
							}
						}
					}
				}
			}
		}

		std::vector<int> CMultiTracker::FindOverlappedObjectsLayers(const std::vector<STrackerInfo*>& trackerInfo) const
		{
			if (trackerInfo.empty())
				return vector<int>();

			// assuming track with largest br() is at the front layer, all the others are at the back layer
			int yMax = 0;
			int topLayerIndex = 0;
			for (size_t i = 0; i < trackerInfo.size(); ++i)
			{
				if (trackerInfo[i]->m_kalman.Measure().br().y > yMax)
				{
					yMax = trackerInfo[i]->m_kalman.Measure().br().y;
					topLayerIndex = i;
				}
			}
			vector<int> layers(trackerInfo.size(), IS_BACK_LAYER);
			layers[topLayerIndex] = IS_FRONT_LAYER;

			return layers;
		}

		bool CMultiTracker::IsObjectStopping(const std::vector<cv::Rect>& boxes) const
		{
			int length = std::min(static_cast<int>(boxes.size()), m_miscellaneousness.m_stopDuration);
			float averageCentreX = 0.0f;
			float averageCentreY = 0.0f;
			float averageWidth = 0.0f;
			float averageHeight = 0.0f;
			vector<Point> centres;
			for (int i = boxes.size() - length; i < static_cast<int>(boxes.size()); ++i)
			{
				Point centre = BoxCentre(boxes[i]);
				averageCentreX += centre.x;
				averageCentreY += centre.y;
				averageWidth += boxes[i].width;
				averageHeight += boxes[i].height;
				centres.push_back(std::move(centre));
			}
			averageCentreX /= length;
			averageCentreY /= length;
			averageWidth /= length;
			averageHeight /= length;

			float varCentreX = 0.0f;
			float varCentreY = 0.0f;
			for (size_t i = 0; i < centres.size(); ++i)
			{
				varCentreX += std::pow(centres[i].x - averageCentreX, 2);
				varCentreY += std::pow(centres[i].y - averageCentreY, 2);
			}
			varCentreX /= (length - 1);
			varCentreY /= (length - 1);

			if (varCentreX < averageWidth * averageWidth * 0.0625 || varCentreY < averageHeight * averageHeight * 0.0625)
				return true;

			return false;

		}

		void CMultiTracker::UpdateTracks(const std::vector<cv::Rect>& boxes, const cv::Mat& frame, const cv::Mat* fg, const std::vector<cv::Rect>* definedEntries)
		{
			// merge nearby foreground boxes
			auto observations = MergeBoxes(boxes, m_miscellaneousness.m_extension);
			// removed small boxes
			observations.erase(std::remove_if(observations.begin(), observations.end(), boost::bind(&CMultiTracker::IsInvalidBox, this, _1)), observations.end());

			if (static_cast<int>(observations.size()) - static_cast<int>(m_tracks.size()) > m_miscellaneousness.m_maxAllowedTracks)
				observations.clear();	// too many observations, maybe outliers

			vector<int> rowAssoc(observations.size(), 0);
			vector<vector<int> > assocMatrix(m_tracks.size(), rowAssoc);
			int newTrackCount = 0;
			if (m_tracks.size() == 0 && observations.size() != 0)
			{
				for (const auto& box: observations)
				{
					InitTrack(box);
					newTrackCount++;
				}
			}
			else if (m_tracks.size() != 0)
			{
				// kalman prediction
				for (auto& track: m_tracks)
				{
					track->m_kalman.Measure();
					if (track->m_meanshift.HasInit())
						track->m_meanshift.SetMeasureUpdated(false);
				}

				// data association
				// if no observations are found in the current frame, every unexited tracks will be
				// propagated using its previous information in kalman filter
				vector<float> rowCost(observations.size(), 0.0f);
				vector<vector<float> > costMatrix(m_tracks.size(), rowCost);
				vector<int> rowStatus(m_tracks.size(), 0);
				vector<vector<int> > statusMatrix(m_tracks.size(), rowStatus);

				if (!observations.empty())
				{
					// initialise cost matrix
					CalcCostMatrix(observations, &costMatrix);

					// initialise association matrix (numTracks x numMergedBoxes)
					LinearAssignment(costMatrix, m_dataAssocParam.m_costDistMax, assocMatrix);

					// to avoid object split into several small parts caused by false detection.
					// we place previous object box in the predicted position, all small
					// unassociated FGs within and near it will be merged to a single object
					MergeSplittedObject(&observations, &assocMatrix, Size(0, 0));

					CalcStatusMatrix(observations, &assocMatrix, &statusMatrix);

					// add new tracks for unassinged observations
					for (size_t i = 0; i < observations.size(); ++i)
					{
						bool isAssigned = std::any_of(assocMatrix.begin(), assocMatrix.end(), [i](const vector<int>& rowAssoc){ return rowAssoc[i] == 0 ? false : true; });
						if (!isAssigned)
						{
							InitTrack(observations[i]);
							newTrackCount++;
						}
					}
				}

				/*************explicit tracking update begin*************/
				//auto assocMatrixBackup = assocMatrix;

				vector<int> assocTracksInd;
				for (size_t i = 0; i < assocMatrix.size(); ++i)
				{
					if (std::any_of(assocMatrix[i].begin(), assocMatrix[i].end(), [](int i){ return i == 1; }))
						assocTracksInd.push_back(i);
				}

				if (!assocTracksInd.empty())
				{
					for (size_t i = 0; i < assocTracksInd.size(); ++i)
					{
						int trackInd = assocTracksInd[i];
						auto track = m_tracks[trackInd];
						int obsInd = std::distance(assocMatrix[trackInd].begin(), std::find(assocMatrix[trackInd].begin(), assocMatrix[trackInd].end(), 1));

						// find all overlapped tracks associated with trackInd
						vector<int> overlappedTracksInd;
						for (size_t j = 0; j < statusMatrix[0].size(); ++j)
						{
							if (statusMatrix[trackInd][j] == IS_OVERLAPPED)
								overlappedTracksInd.push_back(j);
						}

						// only do meanshift for non-entering tracks, as the size of tracks in the entering changes significantly.
						if (!track->m_status.HasStatus(SObjectStatus::Entering/* | SObjectStatus::Exiting*/))
						{
							if (overlappedTracksInd.empty()) // track has no overlapped track
							{
								if (!track->m_meanshift.HasInit())
									track->m_meanshift.Init(observations[obsInd], frame, fg);
								else
									track->m_meanshift.Update(observations[obsInd], frame, fg);
							}
							else // track has overlapped tracks
							{
								// find the layers of all overlapped tracks
								vector<STrackerInfo*> tmpTrackerInfo;
								tmpTrackerInfo.push_back(track);

								track->m_status.AddStatus(SObjectStatus::Overlapping);
								for (size_t k = 0; k < overlappedTracksInd.size(); ++k)
								{
									tmpTrackerInfo.push_back(m_tracks[overlappedTracksInd[k]]);
									m_tracks[overlappedTracksInd[k]]->m_status.AddStatus(SObjectStatus::Overlapping);
								}

								auto layerInfo = FindOverlappedObjectsLayers(tmpTrackerInfo);
								int frontLayerIndex = std::distance(layerInfo.begin(), std::find(layerInfo.begin(), layerInfo.end(), IS_FRONT_LAYER));
								bool IsFrontLayerMeanShiftInit = tmpTrackerInfo[frontLayerIndex]->m_meanshift.HasInit();
								bool noMeanShiftTracking = false;
								if (IsFrontLayerMeanShiftInit)
								{
									for (size_t k = 0; k < layerInfo.size(); ++k)
									{
										if (k != frontLayerIndex && tmpTrackerInfo[k]->m_meanshift.HasInit())
										{
											auto BHValue = CMeanShiftFilter::CalcBhattacharyya(tmpTrackerInfo[frontLayerIndex]->m_meanshift.Histogram(), tmpTrackerInfo[frontLayerIndex]->m_meanshift.HistogramVolumn(), tmpTrackerInfo[k]->m_meanshift.Histogram(), tmpTrackerInfo[k]->m_meanshift.HistogramVolumn());
											if (BHValue > CMeanShiftFilter::BHCoeffThreshold())  // overlapped tracks colour are similar, disable meanshift temporally
												noMeanShiftTracking = true;
										}
									}

									if (!noMeanShiftTracking)
										tmpTrackerInfo[frontLayerIndex]->m_meanshift.Update(tmpTrackerInfo[frontLayerIndex]->m_kalman.Measure(), frame, fg);
								}
							}
						}

						if (overlappedTracksInd.empty() && track->m_status.HasStatus(SObjectStatus::Overlapping))
							track->m_status.RemoveStatus(SObjectStatus::Overlapping);
						else if (track->m_status.HasStatus(SObjectStatus::Lost))
							track->m_status.RemoveStatus(SObjectStatus::Lost);
					}
				}
				for (size_t i = 0; i < m_tracks.size() - newTrackCount; ++i)
				{
					if (std::find(assocTracksInd.begin(), assocTracksInd.end(), i) == assocTracksInd.end() && !m_tracks[i]->m_status.HasStatus(SObjectStatus::Overlapping))
					{
						m_tracks[i]->m_status.AddStatus(SObjectStatus::Lost);

						if (m_tracks[i]->m_meanshift.HasInit() && !m_tracks[i]->m_status.HasStatus(SObjectStatus::Exiting) && IsObjectStopping(m_tracks[i]->m_features.m_boundingBoxes))
						{
							Rect newMeasure(m_tracks[i]->m_kalman.Measure().x, m_tracks[i]->m_kalman.Measure().y, m_tracks[i]->m_features.m_latestBoundingBox.width, m_tracks[i]->m_features.m_latestBoundingBox.height);
							m_tracks[i]->m_meanshift.Update(newMeasure, frame, fg);
						}
						else if (m_tracks[i]->m_status.HasStatus(SObjectStatus::Exiting))
							m_tracks[i]->m_removable = true;

					}
				}
			}

			// update and store tracks bounding box at different status
			for (size_t i = 0; i < m_tracks.size() - newTrackCount; ++i) // for existing tracks
			{
				// find the foreground box
				int index = std::distance(assocMatrix[i].begin(), std::find(assocMatrix[i].begin(), assocMatrix[i].end(), 1));
				const Rect* box = nullptr;
				if (index != assocMatrix[i].size())
					box = &observations[index];

				auto& track = m_tracks[i];
				if (box && track->m_meanshift.HasMeasureUpdated())
				{
					if (track->m_status.HasStatus(SObjectStatus::Overlapping))	// do not update size
						track->m_features.m_latestBoundingBox = track->m_meanshift.Measure();
					else
					{
						Point centre = BoxCentre(track->m_meanshift.Measure());
						int height = static_cast<int>(track->m_features.m_latestBoundingBox.height * (1 - m_miscellaneousness.m_objSizeUpdateRate) + box->height * m_miscellaneousness.m_objSizeUpdateRate);
						int width = static_cast<int>(track->m_features.m_latestBoundingBox.width * (1 - m_miscellaneousness.m_objSizeUpdateRate) + box->width * m_miscellaneousness.m_objSizeUpdateRate);
						track->m_features.m_latestBoundingBox = Rect(centre.x - width / 2, centre.y - height / 2, width, height);
					}
				}
				else if(box && !track->m_meanshift.HasMeasureUpdated())
				{
					Point centre = BoxCentre(*box);
					int height = static_cast<int>(track->m_features.m_latestBoundingBox.height * (1 - m_miscellaneousness.m_objSizeUpdateRate) + box->height * m_miscellaneousness.m_objSizeUpdateRate);
					int width = static_cast<int>(track->m_features.m_latestBoundingBox.width * (1 - m_miscellaneousness.m_objSizeUpdateRate) + box->width * m_miscellaneousness.m_objSizeUpdateRate);
					track->m_features.m_latestBoundingBox = Rect(centre.x - width / 2, centre.y - height / 2, width, height);
				}
				else if (!box && track->m_meanshift.HasMeasureUpdated())
				{
					track->m_features.m_latestBoundingBox = track->m_meanshift.Measure();
				}
				else
				{
					track->m_features.m_latestBoundingBox = track->m_kalman.Measure();
				}

				track->m_kalman.Update(track->m_features.m_latestBoundingBox);
			}

			for (size_t i = m_tracks.size() - newTrackCount; i < m_tracks.size(); ++i)
			{
				auto& track = m_tracks[i];
				track->m_features.m_latestBoundingBox = track->m_kalman.Measure();
			}

			for (size_t i = 0; i < m_tracks.size(); ++i)
			{
				auto& track = m_tracks[i];
				track->m_features.m_boundingBoxes.push_back(track->m_features.m_latestBoundingBox);
				track->m_duration++;
				if (track->m_status.HasStatus(SObjectStatus::Overlapping | SObjectStatus::Lost))
				{
					track->m_propagation++;
					track->m_totalPropagation++;
				}
				else
				{
					track->m_propagation = 0;
				}

				track->m_validityScore = 1 - static_cast<float>(track->m_totalPropagation) / track->m_duration;

			}

			////////////////////////////////
			// TODO: HLI handle object split
			////////////////////////////////

			////////////////////////////////
			// TODO: HLI handle object merge
			////////////////////////////////

			for (auto& track: m_tracks)
			{
				if (track->m_duration > m_miscellaneousness.m_minDispDuration)
				{
					track->m_isRecording = true;
					vector<Rect> boxes(track->m_features.m_boundingBoxes.end() - std::min(static_cast<int>(track->m_features.m_boundingBoxes.size()), 3), track->m_features.m_boundingBoxes.end());
					if (!track->m_status.HasStatus(SObjectStatus::In) && !track->m_status.HasStatus(SObjectStatus::Overlapping) &&
						!IsBoxesCloseToEntries(boxes, frame.size(), nullptr))
					{
						track->m_status.AddStatus(SObjectStatus::In);
						track->m_status.RemoveStatus(SObjectStatus::Entering | SObjectStatus::Exiting);
					}
					else if (track->m_status.HasStatus(SObjectStatus::In) &&
							 IsBoxesCloseToEntries(boxes, frame.size(), definedEntries))
					{
						track->m_status.AddStatus(SObjectStatus::Exiting);
						track->m_status.RemoveStatus(SObjectStatus::Entering | SObjectStatus::In);
					}
				}

				// TODO: HLI how to assign stopping status

			}

			// managing the track list
			for (auto & track: m_tracks)
			{
				if (!track->m_removable &&
					((track->m_isRecording && track->m_status.HasStatus(SObjectStatus::Overlapping) && track->m_propagation > CKalmanFilter::MaxOverlapPropagation()) ||
					(track->m_isRecording && track->m_status.HasStatus(SObjectStatus::Lost) && track->m_propagation > CKalmanFilter::MaxLostPropagation()) ||
					(!track->m_isRecording && track->m_propagation > CKalmanFilter::MaxProgrationNoRecord())))
				{
					ReleaseTrack(*track);
					track->m_removable = true;
					continue;
				}

				Point centre = BoxCentre(track->m_features.m_latestBoundingBox);
				if (!track->m_removable && (!CoordinateInFrame(frame.size(), centre.x, EAxis::X) || !CoordinateInFrame(frame.size(), centre.y, EAxis::Y)))
				{
					ReleaseTrack(*track);
					track->m_removable = true;
					continue;
				}

				if (!track->m_removable && (track->m_kalman.Measure().height == 0 || track->m_kalman.Measure().width == 0))
				{
					ReleaseTrack(*track);
					track->m_removable = true;
					continue;
				}
			}

			m_tracks.erase(std::remove_if(m_tracks.begin(), m_tracks.end(), [](const STrackerInfo* trackInfo) { return trackInfo->m_removable; }), m_tracks.end());

			m_frameCount++;
		}

		void CMultiTracker::DisplayTracks(cv::Mat& frame, const cv::Size& extension)
		{
			for (auto& track: m_tracks)
			{
				if (track->m_duration > m_miscellaneousness.m_minDispDuration && track->m_validityScore > 0.6)
				{
					string display;
					const Rect& box = track->m_features.m_latestBoundingBox;
					Point centre = BoxCentre(box);
					int width = box.width + 2 * extension.width;
					int height = box.height + 2 * extension.width;
					Rect displayBox = Rect(centre.x - width / 2, centre.y - height /2, width, height);

					putText(frame, display, displayBox.tl() , FONT_HERSHEY_PLAIN, 1, CVROI::Colour(CVROI::EColour::GREEN));
					rectangle(frame, displayBox.tl(), displayBox.br(), CVROI::Colour(CVROI::EColour::GREEN), 1);

					// draw most recent trajectory
					int startIndex = track->m_duration >= 50 ? track->m_duration - 50 : 0;
					for (int i = startIndex; i < track->m_duration - 1; ++i)
					{
						line(frame, BoxCentre(track->m_features.m_boundingBoxes[i]), BoxCentre(track->m_features.m_boundingBoxes[i+1]), CVROI::Colour(CVROI::EColour::GREEN));
					}
				}
			}
		}

		//void CMultiTracker::WriteToXML() const
		//{
		//
		//}

    }
}


#include "cnn/lstm.h"
#include <boost/lexical_cast.hpp>
#include <string>
#include <cassert>
#include <vector>
#include <iostream>

#include "cnn/nodes.h"

using namespace std;
using namespace cnn::expr;

namespace cnn {

enum { X2I, H2I, C2I, BI, X2O, H2O, C2O, BO, X2C, H2C, BC };

LSTMBuilder::LSTMBuilder(unsigned ilayers,
                         const vector<unsigned>& dims,
                         Model* model,
                         cnn::real iscale,
                         string name)
{
  unsigned input_dim = dims[INPUT_LAYER];
  unsigned hidden_dim = dims[HIDDEN_LAYER];
  layers = ilayers;
  string i_name;
  unsigned layer_input_dim = input_dim;
  input_dims = vector<unsigned>(layers, layer_input_dim);

  for (unsigned i = 0; i < layers; ++i) {
    input_dims[i] = layer_input_dim;

    // i
    if (name.size() > 0)
        i_name = name + "p_x2i" + boost::lexical_cast<string>(i);
    Parameters* p_x2i = model->add_parameters({ hidden_dim, layer_input_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_h2i" + boost::lexical_cast<string>(i);
    Parameters* p_h2i = model->add_parameters({ hidden_dim, hidden_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_c2i" + boost::lexical_cast<string>(i);
    Parameters* p_c2i = model->add_parameters({ hidden_dim, hidden_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_bi" + boost::lexical_cast<string>(i);
    Parameters* p_bi = model->add_parameters({ hidden_dim }, iscale, i_name);

    // o
    if (name.size() > 0)
        i_name = name + "p_x2o" + boost::lexical_cast<string>(i);
    Parameters* p_x2o = model->add_parameters({ hidden_dim, layer_input_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_h2o" + boost::lexical_cast<string>(i);
    Parameters* p_h2o = model->add_parameters({ hidden_dim, hidden_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_c2o" + boost::lexical_cast<string>(i);
    Parameters* p_c2o = model->add_parameters({ hidden_dim, hidden_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_bo" + boost::lexical_cast<string>(i);
    Parameters* p_bo = model->add_parameters({ hidden_dim }, iscale, i_name);

    // c
    if (name.size() > 0)
        i_name = name + "p_x2c" + boost::lexical_cast<string>(i);
    Parameters* p_x2c = model->add_parameters({ hidden_dim, layer_input_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_h2c" + boost::lexical_cast<string>(i);
    Parameters* p_h2c = model->add_parameters({ hidden_dim, hidden_dim }, iscale, i_name);
    if (name.size() > 0)
        i_name = name + "p_bc" + boost::lexical_cast<string>(i);
    Parameters* p_bc = model->add_parameters({ hidden_dim }, iscale, i_name);
    layer_input_dim = hidden_dim;  // output (hidden) from 1st layer is input to next

    vector<Parameters*> ps = {p_x2i, p_h2i, p_c2i, p_bi, p_x2o, p_h2o, p_c2o, p_bo, p_x2c, p_h2c, p_bc};

#ifdef DBG_MODEL
    for (auto p : ps)
    {
        p->reset_to_zero();
    }
#endif
    params.push_back(ps);
  }  // layers
}

void LSTMBuilder::new_graph_impl(ComputationGraph& cg){
  param_vars.clear();

  for (unsigned i = 0; i < layers; ++i){
    auto& p = params[i];

    //i
    Expression i_x2i = parameter(cg,p[X2I]);
    Expression i_h2i = parameter(cg,p[H2I]);
    Expression i_c2i = parameter(cg,p[C2I]);
    Expression i_bi = parameter(cg,p[BI]);
    //o
    Expression i_x2o = parameter(cg,p[X2O]);
    Expression i_h2o = parameter(cg,p[H2O]);
    Expression i_c2o = parameter(cg,p[C2O]);
    Expression i_bo = parameter(cg,p[BO]);
    //c
    Expression i_x2c = parameter(cg,p[X2C]);
    Expression i_h2c = parameter(cg,p[H2C]);
    Expression i_bc = parameter(cg,p[BC]);

    vector<Expression> vars = {i_x2i, i_h2i, i_c2i, i_bi, i_x2o, i_h2o, i_c2o, i_bo, i_x2c, i_h2c, i_bc};
    param_vars.push_back(vars);

  }
  set_data_in_parallel(data_in_parallel());
}

// layout: 0..layers = c
//         layers+1..2*layers = h
void LSTMBuilder::start_new_sequence_impl(const vector<Expression>& hinit) {
  h.clear();
  c.clear();
  if (hinit.size() > 0) {
    assert(layers*2 == hinit.size());
    h0.resize(layers);
    c0.resize(layers);
    for (unsigned i = 0; i < layers; ++i) {
      c0[i] = hinit[i];
      h0[i] = hinit[i + layers];
    }
    has_initial_state = true;
  } else {
    has_initial_state = false;
  }

  set_data_in_parallel(data_in_parallel());
}

void LSTMBuilder::set_data_in_parallel(int n)
{
    RNNBuilder::set_data_in_parallel(n);

    biases.clear();
    for (unsigned i = 0; i < layers; ++i) {
        const vector<Expression>& vars = param_vars[i];
        Expression bimb = concatenate_cols(vector<Expression>(data_in_parallel(), vars[BI]));
        Expression bcmb = concatenate_cols(vector<Expression>(data_in_parallel(), vars[BC]));
        Expression bomb = concatenate_cols(vector<Expression>(data_in_parallel(), vars[BO]));
        vector<Expression> b = { bimb, bcmb, bomb };
        biases.push_back(b);
    }
}

Expression LSTMBuilder::add_input_impl(const vector<Expression>& prev_history, const Expression &x)
{
    h.push_back(vector<Expression>(layers));
    c.push_back(vector<Expression>(layers));
    vector<Expression>& ht = h.back();
    vector<Expression>& ct = c.back();
    Expression in = x;

    for (unsigned i = 0; i < layers; ++i) {
        const vector<Expression>& vars = param_vars[i];
        Expression i_h_tm1, i_c_tm1;
        i_h_tm1 = prev_history[i+layers];
        i_c_tm1 = prev_history[i];

        // input
        Expression i_ait;
        Expression bimb = biases[i][0];
        if (prev_history.size() > 0)
            i_ait = affine_transform({ bimb, vars[X2I], in, vars[H2I], i_h_tm1, vars[C2I], i_c_tm1 });
        else
            i_ait = affine_transform({ bimb, vars[X2I], in});

        Expression i_it = logistic(i_ait);
        // forget
        Expression i_ft = 1.f - i_it;

        // write memory cell
        Expression i_awt;
        Expression bcmb = biases[i][1];
        if (prev_history.size() > 0)
            i_awt = affine_transform({ bcmb, vars[X2C], in, vars[H2C], i_h_tm1 });
        else
            i_awt = affine_transform({ bcmb, vars[X2C], in });

        Expression i_wt = tanh(i_awt);
        // output
        Expression i_nwt = cwise_multiply(i_it, i_wt);
        if (prev_history.size() > 0)
        {
            Expression i_crt = cwise_multiply(i_ft, i_c_tm1);
            ct[i] = i_crt + i_nwt;
        }
        else
            ct[i] = i_nwt;

        Expression i_aot;
        Expression bomb = biases[i][2];
        if (prev_history.size() > 0)
            i_aot = affine_transform({ bomb, vars[X2O], in, vars[C2O], ct[i] });
        else
            i_aot = affine_transform({ bomb, vars[X2O], in, vars[H2O], i_h_tm1, vars[C2O], ct[i] });

        Expression i_ot = logistic(i_aot);
        Expression ph_t = tanh(ct[i]);
        in = ht[i] = cwise_multiply(i_ot, ph_t);
    }
    return ht.back();
}

Expression LSTMBuilder::add_input_impl(int prev, const Expression& x)
{
  h.push_back(vector<Expression>(layers));
  c.push_back(vector<Expression>(layers));
  vector<Expression>& ht = h.back();
  vector<Expression>& ct = c.back();
  Expression in = x;

  for (unsigned i = 0; i < layers; ++i) {
    const vector<Expression>& vars = param_vars[i];
    Expression i_h_tm1, i_c_tm1;
    bool has_prev_state = (prev >= 0 || has_initial_state);
    if (prev < 0) {
      if (has_initial_state) {
        // intial value for h and c at timestep 0 in layer i
        // defaults to zero matrix input if not set in add_parameter_edges
        i_h_tm1 = h0[i];
        i_c_tm1 = c0[i];
      }
    } else {  // t > 0
      i_h_tm1 = h[prev][i];
      i_c_tm1 = c[prev][i];
    }

    // input
    Expression i_ait;
    Expression bimb = biases[i][0];
    if (has_prev_state)
//      i_ait = vars[BI] + vars[X2I] * in + vars[H2I]*i_h_tm1 + vars[C2I] * i_c_tm1;
      i_ait = affine_transform({bimb, vars[X2I], in, vars[H2I], i_h_tm1, vars[C2I], i_c_tm1});
    else
//      i_ait = vars[BI] + vars[X2I] * in;
      i_ait = affine_transform({ bimb, vars[X2I], in });
    Expression i_it = logistic(i_ait);
    // forget
    Expression i_ft = 1.f - i_it;

    // write memory cell
    Expression i_awt;
    Expression bcmb = biases[i][1];
    if (has_prev_state)
//      i_awt = vars[BC] + vars[X2C] * in + vars[H2C]*i_h_tm1;
      i_awt = affine_transform({ bcmb, vars[X2C], in, vars[H2C], i_h_tm1 });
    else
//      i_awt = vars[BC] + vars[X2C] * in;
      i_awt = affine_transform({ bcmb, vars[X2C], in });
    Expression i_wt = tanh(i_awt);
    // output
    if (has_prev_state) {
      Expression i_nwt = cwise_multiply(i_it,i_wt);
      Expression i_crt = cwise_multiply(i_ft,i_c_tm1);
      ct[i] = i_crt + i_nwt;
    } else {
      ct[i] = cwise_multiply(i_it,i_wt);
    }

    Expression i_aot;
    Expression bomb = biases[i][2];
    if (has_prev_state)
//      i_aot = vars[BO] + vars[X2O] * in + vars[H2O] * i_h_tm1 + vars[C2O] * ct[i];
      i_aot = affine_transform({ bomb, vars[X2O], in, vars[H2O], i_h_tm1, vars[C2O], ct[i] });
    else
      i_aot = affine_transform({ bomb, vars[X2O], in });
    Expression i_ot = logistic(i_aot);
    Expression ph_t = tanh(ct[i]);
    in = ht[i] = cwise_multiply(i_ot,ph_t);
  }
  return ht.back();
}

Expression LSTMBuilder::add_input_impl(int prev, const vector<Expression>& x)
{
    h.push_back(vector<Expression>(layers));
    c.push_back(vector<Expression>(layers));
    vector<Expression>& ht = h.back();
    vector<Expression>& ct = c.back();

    assert(x.size() == layers);
    Expression in = x[0];

    for (unsigned i = 0; i < layers; ++i) {
        const vector<Expression>& vars = param_vars[i];
        Expression i_h_tm1, i_c_tm1;
        bool has_prev_state = (prev >= 0 || has_initial_state);
        if (prev < 0) {
            if (has_initial_state) {
                // intial value for h and c at timestep 0 in layer i
                // defaults to zero matrix input if not set in add_parameter_edges
                i_h_tm1 = h0[i];
                i_c_tm1 = c0[i];
            }
        }
        else {  // t > 0
            i_h_tm1 = h[prev][i];
            i_c_tm1 = c[prev][i];
        }

        // input
        Expression i_ait;
        Expression bimb = biases[i][0];
        if (has_prev_state)
            //      i_ait = vars[BI] + vars[X2I] * in + vars[H2I]*i_h_tm1 + vars[C2I] * i_c_tm1;
            i_ait = affine_transform({ bimb, vars[X2I], in, vars[H2I], i_h_tm1, vars[C2I], i_c_tm1 });
        else
            //      i_ait = vars[BI] + vars[X2I] * in;
            i_ait = affine_transform({ bimb, vars[X2I], in });
        Expression i_it = logistic(i_ait);
        // forget
        Expression i_ft = 1.f - i_it;

        // write memory cell
        Expression i_awt;
        Expression bcmb = biases[i][1];
        if (has_prev_state)
            //      i_awt = vars[BC] + vars[X2C] * in + vars[H2C]*i_h_tm1;
            i_awt = affine_transform({ bcmb, vars[X2C], in, vars[H2C], i_h_tm1 });
        else
            //      i_awt = vars[BC] + vars[X2C] * in;
            i_awt = affine_transform({ bcmb, vars[X2C], in });
        Expression i_wt = tanh(i_awt);
        // output
        if (has_prev_state) {
            Expression i_nwt = cwise_multiply(i_it, i_wt);
            Expression i_crt = cwise_multiply(i_ft, i_c_tm1);
            ct[i] = i_crt + i_nwt;
        }
        else {
            ct[i] = cwise_multiply(i_it, i_wt);
        }

        Expression i_aot;
        Expression bomb = biases[i][2];
        if (has_prev_state)
            //      i_aot = vars[BO] + vars[X2O] * in + vars[H2O] * i_h_tm1 + vars[C2O] * ct[i];
            i_aot = affine_transform({ bomb, vars[X2O], in, vars[H2O], i_h_tm1, vars[C2O], ct[i] });
        else
            i_aot = affine_transform({ bomb, vars[X2O], in });
        Expression i_ot = logistic(i_aot);
        Expression ph_t = tanh(ct[i]);

        Expression z = cwise_multiply(i_ot, ph_t);
        if (i > 0)
            z = z + x[i];
        in = ht[i] = z;
    }
    return ht.back();
}
void LSTMBuilder::copy(const RNNBuilder & rnn) {
  const LSTMBuilder & rnn_lstm = (const LSTMBuilder&)rnn;
  assert(params.size() == rnn_lstm.params.size());
  for(size_t i = 0; i < params.size(); ++i)
      for(size_t j = 0; j < params[i].size(); ++j)
        params[i][j]->copy(*rnn_lstm.params[i][j]);
}

} // namespace cnn


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_CRC_HPP
#define BOOST_HASH_CRC_HPP

#include <boost/crc.hpp>
#include <boost/hash/detail/primes.hpp>
#include <boost/hash/digest.hpp>
#include <boost/hash/pack.hpp>
#include <boost/static_assert.hpp>

#include <climits>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

namespace boost {
namespace hashes {

// Boost.CRC undefs this, so re-define it
#define BOOST_CRC_PARM_TYPE  typename ::boost::uint_t<Bits>::fast

template <unsigned Bits, BOOST_CRC_PARM_TYPE TruncPoly = 0u,
          BOOST_CRC_PARM_TYPE InitRem = 0u, BOOST_CRC_PARM_TYPE FinalXor = 0u,
          bool ReflectIn = false, bool ReflectRem = false >
class basic_crc {
  public:
    typedef crc_optimal<Bits, TruncPoly,
                        InitRem, FinalXor,
                        ReflectIn, ReflectRem> crc_computer;
    typedef typename crc_computer::value_type word_type;

    static unsigned const value_bits = CHAR_BIT;
    typedef uint_t<value_bits>::least value_type;

    BOOST_STATIC_ASSERT(Bits >= value_bits);

    static unsigned const digest_bits = Bits;
    typedef hashes::digest<digest_bits> digest_type;

  public:
    basic_crc() { reset(); }
    void reset() { crc_.reset(); }

    digest_type digest() const {
        word_type x = crc_.checksum();
        digest_type d;
        // TODO: Justify bit order
        pack_n<stream_endian::big_bit,
               digest_bits,
               octet_bits>(&x, 1,
                           d.data(), digest_bits/octet_bits);
        return d;
    }

    digest_type end_message() {
        digest_type d = digest();
        reset();
        return d;
    }

  public:
    basic_crc &
    update_one(value_type x) {
#ifdef BOOST_HASH_SHOW_PROGRESS
printf("%.8lx + %.2x ==> ", (long)crc_.checksum(), (int)x);
#endif
        crc_.process_byte(x);
#ifdef BOOST_HASH_SHOW_PROGRESS
printf("%.8lx\n", (long)crc_.checksum());
#endif
        return *this;
    }

    template <typename IterT>
    basic_crc &
    update_n(IterT p, size_t n) {
        while (n--) update_one(*p++);
        return *this;
    }
#ifndef BOOST_HASH_NO_OPTIMIZATION
    template <typename ValT>
    basic_crc &
    update_n(ValT const *p, size_t n) {
        if (sizeof(ValT) == 1) {
            crc_.process_bytes(p, n);
        } else {
            while (n--) update_one(*p++);
        }
        return *this;
    }
    template <typename ValT>
    basic_crc &
    update_n(ValT *p, size_t n) {
        return update_n((ValT const *)p, n);
    }
#endif

    template <typename IterT>
    basic_crc &
    update(IterT b, IterT e, std::random_access_iterator_tag) {
        return update_n(b, e-b);
    }
    template <typename IterT, typename Category>
    basic_crc &
    update(IterT b, IterT e, Category) {
        while (b != e) update_one(*b++);
        return *this;
    }
    template <typename IterT>
    basic_crc &
    update(IterT b, IterT e) {
        typedef typename std::iterator_traits<IterT>::iterator_category cat;
        return update(b, e, cat());
    }

  private:
     crc_computer crc_;
};

template <unsigned Bits, BOOST_CRC_PARM_TYPE TruncPoly = 0u,
          BOOST_CRC_PARM_TYPE InitRem = 0u, BOOST_CRC_PARM_TYPE FinalXor = 0u,
          bool ReflectIn = false, bool ReflectRem = false >
struct crc {
  private:
    typedef basic_crc<Bits, TruncPoly, InitRem, FinalXor, ReflectIn, ReflectRem>
            octet_hash_type;
  public:
    template <unsigned value_bits>
    struct stream_hash {
        BOOST_STATIC_ASSERT(value_bits == CHAR_BIT);
        typedef octet_hash_type type_;
#ifdef BOOST_HASH_NO_HIDE_INTERNAL_TYPES
        typedef type_ type;
#else
        struct type : type_ {};
#endif
    };
    typedef typename octet_hash_type::digest_type digest_type;
};

// http://www.libpng.org/pub/png/spec/1.2/PNG-Structure.html#CRC-algorithm
typedef crc<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> crc32_png;

} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_CRC_HPP