void quickWorldModelTest() {
    gSystem->Load("lib/libPropagation.so");
//    BedmapTable *iceTable = new BedmapTable (RyansCons::kBathymetry);
    WorldModel *myWorld = new WorldModel();
    TH2F *iceHist = new TH2F("iceHist","Ice Thickness",1000,-3e6,3e6,1000,-3e6,3e6);
    TH2F *iceHist2 = new TH2F("iceHist2","Ice Thickness (Bedmap)",1000,-3e6,3e6,1000,-3e6,3e6);
    TH2F *surfaceHist = new TH2F("surfaceHist","Surface",1000,-3e6,3e6,1000,-3e6,3e6);
//    int goodFlag;
    for(int binx=1;binx<=iceHist->GetNbinsX();binx++) {
	Double_t x=iceHist->GetXaxis()->GetBinCenter(binx);
	for(int biny=1;biny<=iceHist->GetNbinsY();biny++) {
	    Double_t y=iceHist->GetYaxis()->GetBinCenter(biny);
	    Double_t value=myWorld->getCrust2Data(x,y,RyansCons::kThicknessOfIce);
	    iceHist->SetBinContent(binx,biny,value);
	    value=myWorld->getIceThickness(x,y);
	    iceHist2->SetBinContent(binx,biny,value);
	    value=myWorld->getSurface(x,y);
	    surfaceHist->SetBinContent(binx,biny,value/1e6);
	    if(value>1e7 || value<1e6) cout << x << "\t" << y << "\t" << value << endl;
	}
    }
    TCanvas *can = new TCanvas("can","can");
    can->Divide(1,2);
    can->cd(1);
    iceHist->Draw("colz");
    can->cd(2);
    iceHist2->Draw("colz");
	
    TCanvas *can2 = new TCanvas("can2","can2");
    surfaceHist->Draw("colz");
    
}

    //=================================================================
    bool TileAssembler::convertRawFile(const std::string& pModelFilename)
    {
        bool success = true;
        std::string filename = iSrcDir;
        if (filename.length() >0)
            filename.push_back('/');
        filename.append(pModelFilename);

        WorldModel_Raw raw_model;
        if (!raw_model.Read(filename.c_str()))
            return false;

        // write WorldModel
        WorldModel model;
        model.setRootWmoID(raw_model.RootWMOID);
        if (!raw_model.groupsArray.empty())
        {
            std::vector<GroupModel> groupsArray;

            uint32 groups = raw_model.groupsArray.size();
            for (uint32 g = 0; g < groups; ++g)
            {
                GroupModel_Raw& raw_group = raw_model.groupsArray[g];
                groupsArray.push_back(GroupModel(raw_group.mogpflags, raw_group.GroupWMOID, raw_group.bounds ));
                groupsArray.back().setMeshData(raw_group.vertexArray, raw_group.triangles);
                groupsArray.back().setLiquidData(raw_group.liquid);
            }

            model.setGroupModels(groupsArray);
        }

        success = model.writeFile(iDestDir + "/" + pModelFilename + ".vmo");
        //std::cout << "readRawFile2: '" << pModelFilename << "' tris: " << nElements << " nodes: " << nNodes << std::endl;
        return success;
    }

/*!

 */
void
CrossGenerator::updateOpponents( const WorldModel & wm )
{
    const double opponent_dist_thr2 = std::pow( 20.0, 2 );

    const Vector2D goal = ServerParam::i().theirTeamGoalPos();
    const AngleDeg goal_angle_from_ball = ( goal - M_first_point ).th();

    for ( AbstractPlayerCont::const_iterator
              p = wm.theirPlayers().begin(),
              end = wm.theirPlayers().end();
          p != end;
          ++p )
    {
        AngleDeg opponent_angle_from_ball = ( (*p)->pos() - M_first_point ).th();
        if ( ( opponent_angle_from_ball - goal_angle_from_ball ).abs() > 90.0 )
        {
            continue;
        }

        if ( (*p)->pos().dist2( M_first_point ) > opponent_dist_thr2 )
        {
            continue;
        }

        M_opponents.push_back( *p );

#ifdef DEBUG_PRINT
        dlog.addText( Logger::PASS,
                      "Cross opponent %d pos(%.1f %.1f)",
                      (*p)->unum(),
                      (*p)->pos().x, (*p)->pos().y );
#endif
    }
}

HOBJECT WorldModel::AttachObject( HOBJECT hObj )
{
	if( !hObj ) return LTNULL;

	// Make sure the object is detached first.
	DetachObject( hObj );

	LTVector	vPos, vParentPos;
	LTRotation	rRot, rParentRot;

	// Get our position and rotation

	g_pLTServer->GetObjectPos( m_hObject, &vParentPos );
	g_pLTServer->GetObjectRotation( m_hObject, &rParentRot );
	
	LTMatrix mRot;
	rParentRot.ConvertToMatrix( mRot );

	// Get the attachment object pos / rot

	g_pLTServer->GetObjectPos( hObj, &vPos );
	g_pLTServer->GetObjectRotation( hObj, &rRot );

	// Calculate the offsets...

	LTVector	vPosOffset( ~mRot * (vPos - vParentPos) );
	LTRotation	rRotOffset = rRot * ~rParentRot;

	// Attach it...

	HATTACHMENT	hAttachment;
	LTRESULT	LTRes = g_pLTServer->CreateAttachment( m_hObject, hObj, LTNULL, 
													   &vPosOffset, &rRotOffset, &hAttachment );
	if( LTRes != LT_OK )
		return NULL;

	LTObjRefNotifier ref( *this );
	ref = hObj;
	m_AttachmentList.push_back( ref );

	// If the object is a WorldModel set it's activate parent...

	if( IsWorldModel( hObj ))
	{
		WorldModel *pWorldModel = dynamic_cast<WorldModel*>(g_pLTServer->HandleToObject( hObj ));
		if( pWorldModel )
		{
			// Send any activate messages the WorldModel recevies to us...

			pWorldModel->SetActivateParent( m_hObject );

			// Add the WorldModel to our list of objects to inherit our ActivateType...

			m_ActivateTypeHandler.InheritObject( hObj );
		}
	}

	return hObj;
}

void lidarlocalizorapp(){
  printf("starting\n");
 
  Lidar lidar;
  lidar.init("lidar/lidarconfig.conf");
  
  LidarManager lidarManager;
  lidarManager.lidar=&lidar; 
  
  lidarManager.init("lidar/lidarpositionconfig.conf");
  
  
  LidarLocalizer localizer;
  localizer.loadPresetMap();
  
  WorldModel model;
  model.init("indoorlidarconf.conf");
  model.lidar=&lidarManager;
  model.lidarLocalizer=&localizer;
  
  Network network;  
  model.network=&network;
  
	Display display=Display(&model);
  
  //start timer
  TimeKeeper::start_time();

  printf("made all objects\n");
  int key = 0;	
  long start=0;
  double x;
  double y;
  try {
	while((key&255)!=27){

	  start=TimeKeeper::GetTime();
	 // printf("loop start\n");
	  
	  model.updateModel();
	//  localizer.estimatePosition(&state,&scanpoints,fake_scan_point_count);

	//  printf("model updated\n");
	  //display.displayModel();
      key=cvWaitKey(15);
	  
	  cout<<"position "<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRot<<endl;
	  cout<<"time elapsed "<<(TimeKeeper::GetTime()-start)/1000000.0<<endl;
	
	}
  }
  catch(...) {
	cerr << "we are catching an uncaught exception. bad things happened" << endl;
  }
  
}

/*!

 */
Strategy::BallArea
Strategy::get_ball_area( const WorldModel & wm )
{
    int ball_step = 1000;
    ball_step = std::min( ball_step, wm.interceptTable()->teammateReachCycle() );
    ball_step = std::min( ball_step, wm.interceptTable()->opponentReachCycle() );
    ball_step = std::min( ball_step, wm.interceptTable()->selfReachCycle() );

    return get_ball_area( wm.ball().inertiaPoint( ball_step ) );
}

/*!

 */
int
TackleGenerator::predictOpponentsReachStep( const WorldModel & wm,
                                            const Vector2D & first_ball_pos,
                                            const Vector2D & first_ball_vel,
                                            const AngleDeg & ball_move_angle )
{
    int first_min_step = 50;

#if 1
    const ServerParam & SP = ServerParam::i();
    const Vector2D ball_end_point = inertia_final_point( first_ball_pos,
                                                         first_ball_vel,
                                                         SP.ballDecay() );
    if ( ball_end_point.absX() > SP.pitchHalfLength()
         || ball_end_point.absY() > SP.pitchHalfWidth() )
    {
        Rect2D pitch = Rect2D::from_center( 0.0, 0.0, SP.pitchLength(), SP.pitchWidth() );
        Ray2D ball_ray( first_ball_pos, ball_move_angle );
        Vector2D sol1, sol2;
        int n_sol = pitch.intersection( ball_ray, &sol1, &sol2 );
        if ( n_sol == 1 )
        {
            first_min_step = SP.ballMoveStep( first_ball_vel.r(), first_ball_pos.dist( sol1 ) );
#ifdef DEBUG_PRINT
            dlog.addText( Logger::CLEAR,
                          "(predictOpponent) ball will be out. step=%d reach_point=(%.2f %.2f)",
                          first_min_step,
                          sol1.x, sol1.y );
#endif
        }
    }
#endif

    int min_step = first_min_step;
    for ( AbstractPlayerCont::const_iterator
              o = wm.theirPlayers().begin(),
              end = wm.theirPlayers().end();
          o != end;
          ++o )
    {
        int step = predictOpponentReachStep( *o,
                                             first_ball_pos,
                                             first_ball_vel,
                                             ball_move_angle,
                                             min_step );
        if ( step < min_step )
        {
            min_step = step;
        }
    }

    return ( min_step == first_min_step
             ? 1000
             : min_step );
}

    bool StaticMapTree::InitMap(const std::string& fname, VMapManager2* vm)
    {
        DEBUG_FILTER_LOG(LOG_FILTER_MAP_LOADING, "Initializing StaticMapTree '%s'", fname.c_str());
        bool success = true;
        std::string fullname = iBasePath + fname;
        FILE* rf = fopen(fullname.c_str(), "rb");
        if (!rf)
            return false;
        else
        {
            char chunk[8];
            // general info
            if (!readChunk(rf, chunk, VMAP_MAGIC, 8)) success = false;
            char tiled;
            if (success && fread(&tiled, sizeof(char), 1, rf) != 1) success = false;
            iIsTiled = !!tiled;
            // Nodes
            if (success && !readChunk(rf, chunk, "NODE", 4)) success = false;
            if (success) success = iTree.readFromFile(rf);
            if (success)
            {
                iNTreeValues = iTree.primCount();
                iTreeValues = new ModelInstance[iNTreeValues];
            }

            if (success && !readChunk(rf, chunk, "GOBJ", 4)) success = false;
            // global model spawns
            // only non-tiled maps have them, and if so exactly one (so far at least...)
            ModelSpawn spawn;
#ifdef VMAP_DEBUG
            DEBUG_LOG("Map isTiled: %u", static_cast<uint32>(iIsTiled));
#endif
            if (!iIsTiled && ModelSpawn::readFromFile(rf, spawn))
            {
                WorldModel* model = vm->acquireModelInstance(iBasePath, spawn.name);
                DEBUG_FILTER_LOG(LOG_FILTER_MAP_LOADING, "StaticMapTree::InitMap(): loading %s", spawn.name.c_str());
                if (model)
                {
                    model->setModelFlags(spawn.flags);
                    // assume that global model always is the first and only tree value (could be improved...)
                    iTreeValues[0] = ModelInstance(spawn, model);
                    iLoadedSpawns[0] = 1;
                }
                else
                {
                    success = false;
                    ERROR_LOG("StaticMapTree::InitMap() could not acquire WorldModel pointer for '%s'!", spawn.name.c_str());
                }
            }

            fclose(rf);
        }
        return success;
    }

/*!

 */
void
Strategy::update( const WorldModel & wm )
{
    static GameTime s_update_time( -1, 0 );

    if ( s_update_time == wm.time() )
    {
        return;
    }
    s_update_time = wm.time();

    updateSituation( wm );
    updatePosition( wm );
}

/*!

 */
void
TackleGenerator::generate( const WorldModel & wm )
{
    static GameTime s_update_time( 0, 0 );

    if ( s_update_time == wm.time() )
    {
        // dlog.addText( Logger::CLEAR,
        //               __FILE__": already updated" );
        return;
    }
    s_update_time = wm.time();

    clear();

    if ( wm.self().isKickable() )
    {
        // dlog.addText( Logger::CLEAR,
        //               __FILE__": kickable" );
        return;
    }

    if ( wm.self().tackleProbability() < 0.001
         && wm.self().foulProbability() < 0.001 )
    {
        // dlog.addText( Logger::CLEAR,
        //               __FILE__": never tacklable" );
        return;
    }

    if ( wm.time().stopped() > 0 )
    {
        // dlog.addText( Logger::CLEAR,
        //               __FILE__": time stopped" );
        return;
    }

    if ( wm.gameMode().type() != GameMode::PlayOn
         && ! wm.gameMode().isPenaltyKickMode() )
    {
        // dlog.addText( Logger::CLEAR,
        //               __FILE__": illegal playmode " );
        return;
    }


#ifdef DEBUG_PROFILE
    MSecTimer timer;
#endif

    calculate( wm );

#ifdef DEBUG_PROFILE
    dlog.addText( Logger::CLEAR,
                  __FILE__": PROFILE. elapsed=%.3f [ms]",
                  timer.elapsedReal() );
#endif

}

QuestTickResult ScanPlanetQuest::Tick(float, const WorldModel &worldModel) {
    int scanned = worldModel.GetMetaData(targetPlanet).GetValue(metaDataMarker);
    if (scanned >= 100) {
        //TODO improve sponsor relationship
        return QuestTickResult(QuestModelAction(QuestActionType::SUCCEED, GetId()));
    }
    return QuestTickResult(GetId());
}

static void CreateServerMark(CLIENTWEAPONFX & theStruct)
{
	// If this isn't a GameBase object, return...

	if (!IsWorldModel(theStruct.hObj)) return;

	// See if we should create a mark, or simply move one of the GameBase's
	// marks.

	// If the GameBase has the max number of marks or this mark is very close
	// to a pre-existing mark, just move that mark to the new position.

    WorldModel* pObj = (WorldModel*) g_pLTServer->HandleToObject(theStruct.hObj);
	if (!pObj) return;

	pObj->CreateServerMark( theStruct );
}

 WorldModel* VMapManager2::acquireModelInstance(const std::string &basepath, const std::string &filename)
 {
     ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
     if (model == iLoadedModelFiles.end())
     {
         WorldModel *worldmodel = new WorldModel();
         if (!worldmodel->readFile(basepath + filename + ".vmo"))
         {
             ERROR_LOG("VMapManager2: could not load '%s%s.vmo'!", basepath.c_str(), filename.c_str());
             delete worldmodel;
             return NULL;
         }
         DEBUG_LOG("VMapManager2: loading file '%s%s'.", basepath.c_str(), filename.c_str());
         model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
         model->second.setModel(worldmodel);
     }
     model->second.incRefCount();
     return model->second.getModel();
 }

void ExplosionView::notify(Subject* s)
{
    WorldModel* m;
    m = dynamic_cast<WorldModel *>(s);    
    if(start_animation)
    {
        float current_radius = timer.GetElapsedTime() * radius_per_second;
        int circle_complete = (current_radius / max_radius) * 100;
        int blue_value = (circle_complete * 255) / 100;
        std::cout << circle_complete << '\n';
        std::cout << blue_value << '\n';
        circle = sf::Shape::Circle(m->get_x_position(), m->get_y_position(), current_radius, sf::Color(255, blue_value, 0));
        app->Draw(circle);
        draw();
        
        if(current_radius > max_radius)
            m->dispose();
        
    }
}

/*!

 */
void
ActionChainHolder::update( const WorldModel & wm )
{
    static GameTime s_update_time( 0, 0 );
    static FieldEvaluator::ConstPtr s_update_evaluator;
    static ActionGenerator::ConstPtr s_update_generator;

    if ( s_update_time == wm.time()
         && s_update_evaluator == M_evaluator
         && s_update_generator == M_generator )
    {
        return;
    }
    s_update_time = wm.time();
    s_update_evaluator = M_evaluator;
    s_update_generator = M_generator;

    M_graph = ActionChainGraph::Ptr( new ActionChainGraph( M_evaluator, M_generator ) );
    M_graph->calculate( wm );
}

/*!

*/
void
DebugClient::writeAll( const WorldModel & world,
                       const ActionEffector & effector )
{
    if ( M_on )
    {
        this->toStr( world, effector );
        if ( M_connected )
        {
            this->send();
        }

        if ( M_write_mode
             && world.time().stopped() == 0 )
        {
            this->write( world.time().cycle() );
        }

        this->clear();
    }
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, std::string("worldmodel"));
	ros::NodeHandle n;

	WorldModel worldmodel;

	ros::Rate loop_rate(1);
	int count = 0;

	while (ros::ok())
	{
		worldmodel.step();

    	ros::spinOnce();

    	loop_rate.sleep();
    	++count;
	}
	return 0;
}

    WorldModel* VMapManager2::acquireModelInstance(const std::string& basepath, const std::string& filename)
    {
        //! Critical section, thread safe access to iLoadedModelFiles
        std::lock_guard<std::mutex> lock(LoadedModelFilesLock);

        ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
        if (model == iLoadedModelFiles.end())
        {
            WorldModel* worldmodel = new WorldModel();
            if (!worldmodel->readFile(basepath + filename + ".vmo"))
            {
                VMAP_ERROR_LOG("misc", "VMapManager2: could not load '%s%s.vmo'", basepath.c_str(), filename.c_str());
                delete worldmodel;
                return NULL;
            }
            VMAP_DEBUG_LOG("maps", "VMapManager2: loading file '%s%s'", basepath.c_str(), filename.c_str());
            model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
            model->second.setModel(worldmodel);
        }
        model->second.incRefCount();
        return model->second.getModel();
    }

    WorldModel* VMapManager2::acquireModelInstance(const std::string& basepath, const std::string& filename)
    {
        //! Critical section, thread safe access to iLoadedModelFiles
        TRINITY_GUARD(ACE_Thread_Mutex, LoadedModelFilesLock);

        ModelFileMap::iterator model = iLoadedModelFiles.find(filename);
        if (model == iLoadedModelFiles.end())
        {
            WorldModel* worldmodel = new WorldModel();
            if (!worldmodel->readFile(basepath + filename + ".vmo"))
            {
                sLog->outError("VMapManager2: could not load '%s%s.vmo'", basepath.c_str(), filename.c_str());
                delete worldmodel;
                return NULL;
            }
            sLog->outDebug(LOG_FILTER_MAPS, "VMapManager2: loading file '%s%s'", basepath.c_str(), filename.c_str());
            model = iLoadedModelFiles.insert(std::pair<std::string, ManagedModel>(filename, ManagedModel())).first;
            model->second.setModel(worldmodel);
        }
        model->second.incRefCount();
        return model->second.getModel();
    }

/*!

 */
void
Strategy::updateSituation( const WorldModel & wm )
{
    M_current_situation = Normal_Situation;

    if ( wm.gameMode().type() != GameMode::PlayOn )
    {
        if ( wm.gameMode().isPenaltyKickMode() )
        {
            dlog.addText( Logger::TEAM,
                          __FILE__": Situation PenaltyKick" );
            M_current_situation = PenaltyKick_Situation;
        }
        else if ( wm.gameMode().isPenaltyKickMode() )
        {
            dlog.addText( Logger::TEAM,
                          __FILE__": Situation OurSetPlay" );
            M_current_situation = OurSetPlay_Situation;
        }
        else
        {
            dlog.addText( Logger::TEAM,
                          __FILE__": Situation OppSetPlay" );
            M_current_situation = OppSetPlay_Situation;
        }
        return;
    }

    int self_min = wm.interceptTable()->selfReachCycle();
    int mate_min = wm.interceptTable()->teammateReachCycle();
    int opp_min = wm.interceptTable()->opponentReachCycle();
    int our_min = std::min( self_min, mate_min );

    if ( opp_min <= our_min - 2 )
    {
        dlog.addText( Logger::TEAM,
                      __FILE__": Situation Defense" );
        M_current_situation = Defense_Situation;
        return;
    }

    if ( our_min <= opp_min - 2 )
    {
        dlog.addText( Logger::TEAM,
                      __FILE__": Situation Offense" );
        M_current_situation = Offense_Situation;
        return;
    }

    dlog.addText( Logger::TEAM,
                  __FILE__": Situation Normal" );
}