/** Returns a vector<int> which for each vector k in V specifies
 * whether it is unique, in which case vector<int>[k] is set to -1, or
 * whether it is a duplicate of another vector l in V, in which case
 * vector<int>[k] is set to the index of l, where 0 <= l <
 * V.size()). */
vector<int> AlphaVectorPlanning::GetDuplicateIndices(const VectorSet &V)
{
    int nrInV=V.size1(), nrS=V.size2();
    bool equal;

    vector<int> duplicates(nrInV,-1);

    for(int i=1;i!=nrInV;++i) // start at 1, first is never a duplicate
    {
        for(int j=0;j!=i;++j) // loop over all previous vectors
        {
            equal=true;
            for(int s=0;s!=nrS;++s)
            {
                if(abs(V(i,s)-V(j,s))>PROB_PRECISION)
                {
                    equal=false;
                    break; // if 1 number differs, they are not equal
                }
            }
            if(equal)
            {
                duplicates[i]=j;
                break; // we need to find only the first duplicate
            }
        }
    }

#if 0 // reduce verbosity
    PrintVectorCout(duplicates); cout << endl;
#endif
    return(duplicates);
}

		/**
		 * Method is used to compose result solid.
		 * @param	faceStatus1 is face status.
		 * @param	faceStatus2 is face status.
		 * @param	faceStatus3 is face status.
		 * @return	pointer to result solid.
		 */
		Solid* GeoModifier::composeSolid(int faceStatus1, int faceStatus2, int faceStatus3) 
		{
			VertexSet* vertices = new VertexSet();
			IntSet* indices = new IntSet();
	
			groupObjectComponents(*firstObject, *vertices, *indices, faceStatus1, faceStatus2);
			groupObjectComponents(*secondObject, *vertices, *indices, faceStatus3, faceStatus3);
	
			VectorSet * vectors = new VectorSet();

			for(int i = 0; i < vertices->GetNumVertices(); i++)
			{
				Vertex * pVertex = vertices->GetVertex(i);
				vectors->push_back(pVertex->getPosition());
			}
	
			Solid* result = new Solid();
			result->indices = *indices;
			result->vertices = *vectors;
			delete indices;
			delete vertices;
			delete vectors;

			return result;
		}

double BeliefValue::GetValue(const BeliefInterface &Belief,
                             const VectorSet &v,
                             const vector<bool> mask)
{
    double x,maxVal=-DBL_MAX;
    bool maskValid=false;
    vector<double> values(v.size2());

    if(mask.size()!=v.size1())
    {
        throw(E("BeliefValue::GetValue: mask has incorrect size"));
    }

    for(unsigned int i=0;i!=v.size1();i++)
    {
        if(mask[i])
        {
            maskValid=true;
            for(unsigned int k=0;k!=v.size2();++k)
                values[k]=v(i,k);

            // compute inner product of belief with vector
            x=Belief.InnerProduct(values);

            // keep the maximizing value
            if(x>maxVal)
                maxVal=x;
        }
    }

    return(maxVal);
}

void ADFun<Base>::ForSparseJacCase(
	const std::set<size_t>&    set_type      ,
	bool                       transpose     ,
	size_t                     q             ,
	const VectorSet&           r             ,
	VectorSet&                 s             )
{	size_t m = Range();

	// check VectorSet is Simple Vector class with sets for elements
	CheckSimpleVector<std::set<size_t>, VectorSet>(
		one_element_std_set<size_t>(), two_element_std_set<size_t>()
	);

	// dimension size of result vector
	if( transpose )
		s.resize(q);
	else	s.resize( m );

	// store results in r and for_jac_sparse_pack_
	CppAD::ForSparseJacSet(
		transpose        , 
		q                , 
		r                ,
		s                ,
		num_var_tape_    ,
		dep_taddr_       ,
		ind_taddr_       ,
		play_            ,
		for_jac_sparse_set_
	);
}

void ADFun<Base>::RevSparseJacCase(
	const std::set<size_t>&      set_type          ,
	bool                         transpose         ,
	bool                         dependency        ,
	size_t                       q                 ,
	const VectorSet&             r                 ,
	VectorSet&                   s                 )
{	// dimension of the result vector
	if( transpose )
		s.resize( Domain() );
	else	s.resize( q );

	// store results in r
	RevSparseJacSet(
		transpose      ,
		dependency     ,
		q              ,
		r              ,
		s              ,
		num_var_tape_  ,
		dep_taddr_     ,
		ind_taddr_     ,
		play_
	);
}

void ADFun<Base>::RevSparseHesCase(
	const std::set<size_t>&   set_type         ,
	bool                      transpose        ,  
	size_t                    q                ,  
	const VectorSet&          s                ,
	VectorSet&                h                )
{	size_t n = Domain();
	if( transpose )
		h.resize(n);
	else	h.resize(q);

	CPPAD_ASSERT_KNOWN( 
		for_jac_sparse_set_.n_set() > 0,
		"RevSparseHes: previous stored call to ForSparseJac did not "
		"use std::set<size_t> for the elements of r."
	);
	CPPAD_ASSERT_UNKNOWN( for_jac_sparse_pack_.n_set() == 0 );
	CPPAD_ASSERT_UNKNOWN( for_jac_sparse_set_.n_set() == num_var_tape_  );
	
	// use sparse_pack for the calculation
	CppAD::RevSparseHesSet( 
		transpose                ,
		q                        ,
		s                        ,
		h                        ,
		num_var_tape_            ,
		dep_taddr_               ,
		ind_taddr_               ,
		play_                    ,
		for_jac_sparse_set_ 
	);
}

VectorSet AlphaVectorPlanning::CrossSum(const VectorSet &A,
                                        const VectorSet &B) const
{
    int nrInA=A.size1(),
        nrInB=B.size1(),
        nrS=A.size1();

#if DEBUG_AlphaVectorPlanning_CrossSum
    cout << "AlphaVectorPlanning::CrossSum of " << nrInA 
         << " times " << nrInB << endl;
#endif

    VectorSet C(nrInA*nrInB,nrS);
    
    int k=-1;
    for(int i=0;i!=nrInA;i++)
        for(int j=0;j!=nrInB;j++)
        {
            k++;
            for(int s=0;s!=nrS;s++)
                C(k,s)=A(i,s)+B(j,s);
        }

    return(C);
}

			/**
			 * Accessor to vertices.
			 * @return	pointer to vertices collection.
			 */
			VectorSet* getVertices()
			{
				VectorSet* newVertices = new VectorSet();

				for(unsigned i = 0; i < vertices.size(); i++)
					newVertices->push_back(vertices[i]);
				return newVertices;
			}

void HexMap::generateMountainRange(mt19937& urng)
{
	static sf::Color mt(128, 88, 44);
	static vector<VectorSet> splat = { { { 1, -1 }, { 2, -1 }, { 0, 0 }, { 1, 0 }, { -1, 1 }, { 0, 1 }, { -2, 2 }, { -1, 2 }, { 0, 2 } },
	{ { 1, -2 }, { 0, -1 }, { 1, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { -1, 1 }, { 0, 1 }, { -1, 2 }, { 0, 2 } },
	{ { 0, -1 }, { 1, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { -1, 1 }, { 0, 1 } },
	{ { 0, -1 }, { 1, -1 }, { 2, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { -1, 1 }, { 0, 1 }, { 1, 1 } },
	{ { 0, -2 }, { 1, -2 }, { -1, -1 }, { 0, -1 }, { 1, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { -1, 1 } },
	{ { -1, -1 }, { 0, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { -2, 1 }, { -1, 1 }, { 0, 1 } },
	{ { -1, 0 }, { 1, 0 }, { 0, 0 }, { -2, 1 }, { -1, 1 }, { 0, 1 }, { 1, 1 }, { -2, 2 }, { -1, 2 }, { 0, 2 } },
	{ { 0, -2 }, { 1, -2 }, { 0, -1 }, { 1, -1 }, { 2, -1 }, { -1, 0 }, { 0, 0 }, { 1, 0 }, { 0, 1 } } };
	sf::VertexArray va;
	static sf::Vector2f w[4];
	sf::Vector2f offset = { (float)rng::getInt(2, 85, urng), (float)rng::getInt(2, 85, urng) };
	for (int a = 0; a < 100; a++) {
		w[0] = { (float)xRange(urng), (float)yRange(urng) };
		if (isAxialInBounds((sf::Vector2i)w[0]) && getAxial((int)w[0].x, (int)w[0].y).hts->FLAGS[HexTileS::WALKABLE]) {
			break;
		}
	}
	w[3] = { (float)rng::radians(urng), (float)rng::getInt(10, 40, urng) };
	polarToCartesian(w[3]);
	w[3] = roundvf(w[0] + w[3]);
	sf::Vector2f avg = { (w[3] + w[0]) / 2.0f };
	// sqrt(a^2 + b^2)
	int dist = (int)sqrtf(powf(abs(w[3].x - w[0].x), 2.0f) + powf(abs(w[3].x - w[0].x), 2.0f));
	for (int a = 1; a < 3; a++) {
		// Distance between the middle points and the endpoint average cannot be greater
		// than the distance between the endpoints, to prevent any super-sharp curves
		w[a] = { (float)rng::radians(urng), (float)rng::getInt(10, clamp(dist, 10, 40), urng) };
		polarToCartesian(w[a]);
		w[a] = roundvf(w[a] + avg);
	}
	float advance = 1.0f / Bezier::lengthCubic(w);
	VectorSet h;
	sf::Vector2f p;
	for (float f = 0.0f; f < 1.0f; f += advance) {
		Bezier::curveCubic(p, f, w);
		if (!isAxialInBounds((sf::Vector2i)p) || !getAxial((int)p.x, (int)p.y).hts->FLAGS[HexTileS::WALKABLE]) {
			break;
		}
		VectorSet& s = splat[rng::getInt(0, splat.size() - 1, urng)];
		for (auto r : s) {
			h.insert((sf::Vector2i)axialToOffset(roundHex((sf::Vector2f)r + p)));
		}
		p = { 0.0f, 0.0f };
	}
	HexTile* cTile = nullptr;
	for (auto& l : h) {
		cTile = &getOffset(l.x, l.y);
		if (!isOffsetInBounds((sf::Vector2i)l) || !cTile->hts->FLAGS[HexTileS::WALKABLE]) {
			continue;
		}
		//pushTileColor((sf::Vector2i)l, sf::Color::White);
		setTileFeature((sf::Vector2i)l, TileFeatureS::get(TileFeatureS::MOUNTAIN), urng);
		cTile->FLAGS[HexTile::MOUNTAINS] = true;
	}
}

void color_general_colpack(
	const VectorSet&        pattern ,
	const VectorSize&       row     ,
	const VectorSize&       col     ,
	CppAD::vector<size_t>&  color   )
{	size_t i, j, k;
	size_t m = pattern.n_set();
	size_t n = pattern.end();

	// Determine number of non-zero entries in each row
	CppAD::vector<size_t> n_nonzero(m);
	size_t n_nonzero_total = 0;
	for(i = 0; i < m; i++)
	{	n_nonzero[i] = 0;
		typename VectorSet::const_iterator pattern_itr(pattern, i);
		j = *pattern_itr;
		while( j != pattern.end() )
		{	n_nonzero[i]++;
			j = *(++pattern_itr);
		}
		n_nonzero_total += n_nonzero[i];
	}

	// Allocate memory and fill in Adolc sparsity pattern
	CppAD::vector<unsigned int*> adolc_pattern(m);
	CppAD::vector<unsigned int>  adolc_memory(m + n_nonzero_total);
	size_t i_memory = 0;
	for(i = 0; i < m; i++)
	{	adolc_pattern[i]    = adolc_memory.data() + i_memory;
		CPPAD_ASSERT_KNOWN(
			std::numeric_limits<unsigned int>::max() >= n_nonzero[i],
			"Matrix is too large for colpack"
		);
		adolc_pattern[i][0] = static_cast<unsigned int>( n_nonzero[i] );
		typename VectorSet::const_iterator pattern_itr(pattern, i);
		j = *pattern_itr;
		k = 1;
		while(j != pattern.end() )
		{
			CPPAD_ASSERT_KNOWN(
				std::numeric_limits<unsigned int>::max() >= j,
				"Matrix is too large for colpack"
			);
			adolc_pattern[i][k++] = static_cast<unsigned int>( j );
			j = *(++pattern_itr);
		}
		CPPAD_ASSERT_UNKNOWN( k == 1 + n_nonzero[i] );
		i_memory += k;
	}
	CPPAD_ASSERT_UNKNOWN( i_memory == m + n_nonzero_total );

	// Must use an external routine for this part of the calculation because
	// ColPack/ColPackHeaders.h has as 'using namespace std' at global level.
	cppad_colpack_general(color, m, n, adolc_pattern);

	return;
}

VectorSet
AlphaVectorPlanning::Prune(const VectorSet &V) const
{
    int nrInV=V.size1(),nrS=V.size2();
    bool dominated,valuesDominated;
    vector<bool> dominatedVectors(nrInV,false);
    int it,it1;
    vector<int> vectorsToKeep;

#if DEBUG_AlphaVectorPlanning_Prune
    cout << "AlphaVectorPlanning::Prune " << nrInV << " vectors" << endl;
#endif

    for(it=0;it!=nrInV;it++)
    {
        it1=0;
        dominated=false;
        // check whether "it" is dominated by any it1
        while(!dominatedVectors[it] && it1!=nrInV && !dominated)
        {
            valuesDominated=true;
            for(int s=0;s!=nrS;s++)
                if(V(it,s) > V(it1,s))
                    valuesDominated=false;

            if(valuesDominated && it1!=it)
                dominated=true;
            else
                dominated=false;

            it1++;
        }
        if(!dominated)
        {
            vectorsToKeep.push_back(it);
#if DEBUG_AlphaVectorPlanning_Prune
            cout << "AlphaVectorPlanning::Prune added vector " << it << endl;
#endif
        }
        else
            dominatedVectors[it]=true;

    }

    int newNrInV=vectorsToKeep.size();
    VectorSet V1(newNrInV,nrS);
    for(int i=0;i!=newNrInV;i++)
        for(int s=0;s!=nrS;s++)
            V1(i,s)=V(vectorsToKeep[i],s);

#if DEBUG_AlphaVectorPlanning_Prune
    cout << "AlphaVectorPlanning::Prune reduced " << nrInV << " to " 
         << newNrInV << endl;
#endif

    return(V1);
}

// Detect contiguous terrain tiles and store them in the regions list
void HexMap::findRegions()
{
	// new tiles to query
	multimap<int, sf::Vector2i> frontier;
	int i = (int)(mapSize_.x*mapSize_.y);
	HexTile* h = nullptr;
	sf::Vector2i p;
	std::unique_ptr<bool> seen(new bool[i]);
	for (int a = 0; a < i; a++) {
		seen.get()[a] = false;
	}
	std::deque<sf::Vector2i> peaks;
	std::deque<Region> regions;

	Region* currentRegion;
	for (int r = 0; r < mapSize_.y; r++) {
		for (int q = 0, qoff = (int)-floor(r / 2.0); q < mapSize_.x; q++, qoff++) {
			i = q + mapSize_.x * r;
			if (seen.get()[i]) {
				continue;
			}
			regions.emplace_back(1, sf::Vector2i(qoff, r));
			currentRegion = &regions.back();
			h = &getAxial(qoff, r);
			frontier.insert(make_pair(0, sf::Vector2i(qoff, r)));
			VectorSet adj;
			while (!frontier.empty()) {
				for (auto f : frontier) {
					clipToBounds(neighbors(f.second, adj));
				}
				frontier.clear();
				for (auto n : adj) {
					p = axialToOffset(n);
					i = p.x + p.y * mapSize_.x;
					if (seen.get()[i]) {
						continue;
					}
					HexTile& t = getAxial(n.x, n.y);
					if (t.height >= 200) {
						peaks.push_back(n);
					}
					if (t.hts == h->hts) {
						frontier.insert(make_pair(0, n));
						seen.get()[i] = true;
						(*currentRegion).size++;
					}
				}
				adj.clear();
			}
		}
	}
}

VectorSet * Solid::getVertices()
{
	// This thing makes a fresh copy and hands the requestor the copy.

	VectorSet * newVertices = new VectorSet();

	for(int i = 0; i < vertices.length(); i++)
	{
		newVertices->AddVector(vertices[i]);
	}

	return newVertices;
}

void color_general_colpack(
    VectorSet&        pattern ,
    const VectorSize&       row     ,
    const VectorSize&       col     ,
    CppAD::vector<size_t>&  color   )
{   size_t i, j, k;
    size_t m = pattern.n_set();
    size_t n = pattern.end();

    // Determine number of non-zero entries in each row
    CppAD::vector<size_t> n_nonzero(m);
    size_t n_nonzero_total = 0;
    for(i = 0; i < m; i++)
    {   n_nonzero[i] = 0;
        pattern.begin(i);
        j = pattern.next_element();
        while( j != pattern.end() )
        {   n_nonzero[i]++;
            j = pattern.next_element();
        }
        n_nonzero_total += n_nonzero[i];
    }

    // Allocate memory and fill in Adolc sparsity pattern
    CppAD::vector<unsigned int*> adolc_pattern(m);
    CppAD::vector<unsigned int>  adolc_memory(m + n_nonzero_total);
    size_t i_memory = 0;
    for(i = 0; i < m; i++)
    {   adolc_pattern[i]    = adolc_memory.data() + i_memory;
        adolc_pattern[i][0] = n_nonzero[i];
        pattern.begin(i);
        j = pattern.next_element();
        k = 1;
        while(j != pattern.end() )
        {   adolc_pattern[i][k++] = j;
            j = pattern.next_element();
        }
        CPPAD_ASSERT_UNKNOWN( k == 1 + n_nonzero[i] );
        i_memory += k;
    }
    CPPAD_ASSERT_UNKNOWN( i_memory == m + n_nonzero_total );

    // Must use an external routine for this part of the calculation because
    // ColPack/ColPackHeaders.h has as 'using namespace std' at global level.
    cppad_colpack_general(color, m, n, adolc_pattern);

    return;
}

void ADFun<Base>::RevSparseHesCase(
	bool              set_type         ,
	bool              transpose        ,  
	size_t            q                ,  
	const VectorSet&  s                ,
	VectorSet&        h                )
{	size_t n = Domain(); 	
	h.resize(q * n );

	CPPAD_ASSERT_KNOWN( 
		for_jac_sparse_pack_.n_set() > 0,
		"RevSparseHes: previous stored call to ForSparseJac did not "
		"use bool for the elements of r."
	);
	CPPAD_ASSERT_UNKNOWN( for_jac_sparse_set_.n_set() == 0 );
	CPPAD_ASSERT_UNKNOWN( for_jac_sparse_pack_.n_set() == total_num_var_ );
	
	// use sparse_pack for the calculation
	CppAD::RevSparseHesBool( 
		transpose                ,
		q                        ,
		s                        ,
		h                        ,
		total_num_var_           ,
		dep_taddr_               ,
		ind_taddr_               ,
		play_                    ,
		for_jac_sparse_pack_ 
	);
}

GaussianJointPdf<V,M>::GaussianJointPdf(
  const char*                  prefix,
  const VectorSet<V,M>& domainSet,
  const V&                     lawExpVector,
  const M&                     lawCovMatrix)
  :
  BaseJointPdf<V,M>(((std::string)(prefix)+"gau").c_str(),domainSet),
  m_lawExpVector     (new V(lawExpVector)),
  m_lawVarVector     (domainSet.vectorSpace().newVector(INFINITY)), // FIX ME
  m_diagonalCovMatrix(false),
  m_lawCovMatrix     (new M(lawCovMatrix))
{
  if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 54)) {
    *m_env.subDisplayFile() << "Entering GaussianJointPdf<V,M>::constructor() [2]"
                            << ": prefix = " << m_prefix
                            << std::endl;
  }

  if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 55)) {
    *m_env.subDisplayFile() << "In GaussianJointPdf<V,M>::constructor()"
                          //<< ", prefix = "            << m_prefix
                            << ": meanVector = "        << this->lawExpVector()
                      << ", Covariance Matrix = " << lawCovMatrix
                            << std::endl;
  }

  if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 54)) {
    *m_env.subDisplayFile() << "Leaving GaussianJointPdf<V,M>::constructor() [2]"
                            << ": prefix = " << m_prefix
                            << std::endl;
  }
}

/**
 * Sets the solid data. An exception may occur in the case of abnormal arrays 
 * (indices making references to inexistent vertices, there are less colors 
 * than vertices...)
 * 
 * @param vertices array of points defining the solid vertices
 * @param indices array of indices for a array of vertices
 * @param colors array of colors defining the vertices colors 
 */
void Solid::setData(VectorSet & vertices, IntSet & indices, ColorSet & colors)
{
	// Clear them...
	//this->vertices = new VectorSet();
	//this->colors = new ColorSet();
	//this->indices = new IntSet();

	//this->vertices.Clear();
	//this->colors.Clear();
	//this->indices.Clear();

	if(indices.length() > 0)
	{
		for(int i=0; i<vertices.length(); i++)
		{
			// Should add cloning to vectorset, and other 2 sets

			this->vertices.AddVector(vertices[i]);
			this->colors.AddColor(colors[i]);
			//this->indices.AddInt(indices[i]);
		}
	
		for(int i=0; i<indices.length(); i++)
		{
			this->indices.AddInt(indices[i]);
		}
	}
}

LogNormalVectorRealizer<V,M>::LogNormalVectorRealizer(const char* prefix,
                                                                    const VectorSet<V,M>& unifiedImageSet,
                                                                    const V& lawExpVector,
                                                                    const M& matU,
                                                                    const V& vecSsqrt,
                                                                    const M& matVt)
  :
  BaseVectorRealizer<V,M>( ((std::string)(prefix)+"gau").c_str(), unifiedImageSet, std::numeric_limits<unsigned int>::max()), // 2011/Oct/02 - Correction thanks to Corey
  m_unifiedLawExpVector  (new V(lawExpVector)),
  m_unifiedLawVarVector  (unifiedImageSet.vectorSpace().newVector( INFINITY)), // FIX ME
  m_lowerCholLawCovMatrix(NULL),
  m_matU                 (new M(matU)),
  m_vecSsqrt             (new V(vecSsqrt)),
  m_matVt                (new M(matVt))
{
  if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 5)) {
    *m_env.subDisplayFile() << "Entering LogNormalVectorRealizer<V,M>::constructor() [2]"
                            << ": prefix = " << m_prefix
                            << std::endl;
  }

  if ((m_env.subDisplayFile()) && (m_env.displayVerbosity() >= 5)) {
    *m_env.subDisplayFile() << "Leaving LogNormalVectorRealizer<V,M>::constructor() [2]"
                            << ": prefix = " << m_prefix
                            << std::endl;
  }
}

BaseScalarFunction<V,M>::BaseScalarFunction(const char* prefix,
    const VectorSet<V,M>& domainSet)
  : m_env(domainSet.env()),
    m_prefix((std::string)(prefix)+"func_"),
    m_domainSet(domainSet)
{
}

//------------------------------set_next_call----------------------------------
void Block::set_next_call( Node *n, VectorSet &next_call, Block_Array &bbs ) {
  if( next_call.test_set(n->_idx) ) return;
  for( uint i=0; i<n->len(); i++ ) {
    Node *m = n->in(i);
    if( !m ) continue;  // must see all nodes in block that precede call
    if( bbs[m->_idx] == this ) 
      set_next_call( m, next_call, bbs );
  }
}