// Copyright (c) 1997  ETH Zurich (Switzerland).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
// 
//
// Author(s)     : Sven Schoenherr <sven@inf.fu-berlin.de>
//                 Bernd Gaertner

#ifndef CGAL_OPTIMISATION_SPHERE_D_H
#define CGAL_OPTIMISATION_SPHERE_D_H

CGAL_BEGIN_NAMESPACE

// Class declarations
// ==================
// general template
template <class Rep_tag, class FT, class RT, class PT, class Traits>
class Optimisation_sphere_d;

template <class FT, class RT, class PT, class Traits>
class Optimisation_sphere_d<Homogeneous_tag, FT, RT, PT, Traits>;

template <class FT, class RT, class PT, class Traits>
class Optimisation_sphere_d<Cartesian_tag, FT, RT, PT, Traits>;
    
CGAL_END_NAMESPACE

    // Class interfaces and implementation
    // ==================================
    // includes


    #include <CGAL/Optimisation/basic.h>

    #include <CGAL/Optimisation/assertions.h>


    CGAL_BEGIN_NAMESPACE

    // Cartesian version
    // -----------------
    template <class FT, class RT, class PT, class Traits>
    class Optimisation_sphere_d<Cartesian_tag, FT, RT, PT, Traits>
    {
    private:
        typedef
          typename Traits::Access_coordinates_begin_d::Coordinate_iterator  It;
        
        // hack needed for egcs, see also test programs
        typedef             FT                              FT_;
        
        int                 d;                      // dimension
        int                 m;                      // |B|
        int                 s;                      // |B\cup S|
        
        FT**                q;                      // the q_j's
        FT***               inv;                    // the A^{-1}_{B^j}'s
        FT*                 v_basis;                // the vector v_B
        
        FT*                 x;                      // solution vector
        FT*                 v;                      // auxiliary vector
        
        FT*                 c;                      // center, for internal use
        PT                  ctr;                    // center, for external use
        FT                  sqr_r;                  // squared_radius
        
        Traits              tco;
        

    public:

        Optimisation_sphere_d& get_sphere (Cartesian_tag)
        {return *this;}

        const Optimisation_sphere_d&
          get_sphere (Cartesian_tag) const
        {return *this;}

        Optimisation_sphere_d (const Traits& t = Traits())
            : d(-1), m(0), s(0), tco (t)
        {}
        
        void set_tco (const Traits& tcobj)
        {
            tco = tcobj;
        }
        
        
        void init (int ambient_dimension)
        {
            d = ambient_dimension;
            m = 0;
            s = 0;
            sqr_r = -FT(1);
        
            q =             new FT*[d+1];
            inv =           new FT**[d+1];
            v_basis =       new FT[d+2];
            x =             new FT[d+2];
            v =             new FT[d+2];
            c =             new FT[d];
        
            for (int j=0; j<d+1; ++j) {
                q[j] =      new FT[d];
                inv[j] =    new FT*[j+2];
                for (int row=0; row<j+2; ++row)
                    inv[j][row] = new FT[row+1];
            }
        
            for (int i=0; i<d; ++i)
                c[i] = FT(0);
            v_basis[0] = FT(1);
        }
        
        
        ~Optimisation_sphere_d ()
        {
            if (d != -1)
               destroy();
        }
        
        
        void destroy ()
        {
            for (int j=0; j<d+1; ++j) {
                for (int row=0; row<j+2; ++row)
                    delete[] inv[j][row];
                delete[] inv[j];
                delete[] q[j];
            }
            delete[] c;
            delete[] v;
            delete[] x;
            delete[] v_basis;
            delete[] inv;
            delete[] q;
        }
        
        
        void set_size (int ambient_dimension)
        {
            if (d != -1)
                destroy();
            if (ambient_dimension != -1)
                init(ambient_dimension);
            else {
                d = -1;
                m = 0;
                s = 0;
            }
        }
        
        
        void push (const PT& p)
        {
            // store q_m = p by copying its cartesian coordinates into q[m]
            It i(tco.access_coordinates_begin_d_object()(p)); FT *o;
            for (o=q[m]; o<q[m]+d; *(o++)=*(i++));
        
            // update v_basis by appending q_m^Tq_m
            v_basis[m+1] = prod(q[m],q[m],d);
        
            if (m==0)
            {
                // set up A^{-1}_{B^0} directly
                FT** M = inv[0];
                M[0][0] = -FT_(2)*v_basis[1];
                M[1][0] = FT_(1);
                M[1][1] = FT_(0);
            } else {
                // set up vector v by computing 2q_j^T q_m, j=0,...,m-1
                v[0] = FT_(1);
                for (int j=0; j<m; ++j)
                    v[j+1] = FT_(2)*prod(q[j],q[m],d);
        
                // compute a_0,...,a_m
                multiply (m-1, v, x);               // x[j]=a_j, j=0,...,m
        
                // compute z
                FT z = FT_(2)*v_basis[m+1] - prod(v,x,m+1);
                CGAL_optimisation_assertion (!CGAL_NTS is_zero (z));
                FT inv_z = FT_(1)/z;
        
                // set up A^{-1}_{B^m}
                FT** M = inv[m-1];          // A^{-1}_B, old matrix
                FT** M_new = inv[m];        // A^{-1}_{B'}, new matrix
        
                // first m rows
                int row, col;
                for (row=0; row<m+1; ++row)
                    for (col=0; col<row+1; ++col)
                        M_new [row][col] = M[row][col] + x[row]*x[col]*inv_z;
        
                // last row
                for (col=0; col<m+1; ++col)
                    M_new [m+1][col] = -x[col]*inv_z;
                M_new [m+1][m+1] = inv_z;
            }
            s = ++m;
            compute_c_and_sqr_r();  // side effect: sets x
        }
        
        
        void pop ()
        {
            --m;
        }
        
        
        FT excess (const PT& p) const
        {
            // compute (c-p)^2
            FT sqr_dist (FT(0));
            It i(tco.access_coordinates_begin_d_object()(p));
            FT *j;
            for (j=c; j<c+d; ++i, ++j)
                sqr_dist += CGAL_NTS square(*i-*j);
            return sqr_dist - sqr_r;
         }
        
        
        
        PT center () const
        {
             return tco.construct_point_d_object()(d, c, c+d);
        }
        
        FT squared_radius () const
        {
             return sqr_r;
        }
        
        int number_of_support_points () const
        {
             return s;
        }
        
        int size_of_basis () const
        {
             return m;
        }
        
        
        bool is_valid (bool verbose = false, int level = true) const
        {
            Verbose_ostream verr (verbose);
            for (int j=1; j<m+1; ++j)
                if (!CGAL_NTS is_positive (x[j]))
                    return (_optimisation_is_valid_fail
                        (verr, "center not in convex hull of support points"));
            return (true);
        }
        
        
    private:
        void multiply (int j, const FT* vec, FT* res)
        {
            FT** M = inv[j];
            for (int row=0; row<j+2; ++row) {
                res[row] = prod(M[row],vec,row+1);
                for (int col = row+1; col<j+2; ++col)
                    res[row] += M[col][row]*vec[col];
            }
        }
        
        
        void compute_c_and_sqr_r ()
        {
            multiply (m-1, v_basis, x);
        
            for (int i=0; i<d; ++i) c[i] = FT(0);
            for (int j=0; j<m; ++j) {
                FT l = x[j+1], *q_j = q[j];
                for (int i=0; i<d; ++i)
                    c[i] += l*q_j[i];
            }
            sqr_r = x[0] + prod(c,c,d);
        }
        
        
        FT prod (const FT* v1, const FT* v2, int k) const
        {
            FT res(FT(0));
            for (const FT *i=v1, *j=v2; i<v1+k; res += (*(i++))*(*(j++)));
            return res;
        }
        
        
    };


    // Homogeneous version
    // -----------------
    template <class FT, class RT, class PT, class Traits>
    class Optimisation_sphere_d<Homogeneous_tag, FT, RT, PT, Traits>
    {
    private:
        typedef
          typename Traits::Access_coordinates_begin_d::Coordinate_iterator  It;
        
        // hack needed for egcs, see also test programs
        typedef             RT                              RT_;
        
        int                 d;                      // dimension
        int                 m;                      // |B|
        int                 s;                      // |B\cup S|
        
        RT**                q;                      // the q_j's
        RT***               inv;                    // the \tilde{A}^{-1}_{B^j}'s
        RT*                 denom;                  // corresponding denominators
        RT**                v_basis;                // the \tilde{v}_B^j
        
        RT*                 x;                      // solution vector
        mutable RT*         v;                      // auxiliary vector
        
        RT*                 c;                      // center, for internal use
        PT                  ctr;                    // center, for external use
        RT                  sqr_r;                  // numerator of squared_radius
        
        Traits              tco;
        
        

    public:

        Optimisation_sphere_d& get_sphere (Homogeneous_tag t)
        {return *this;}

        const Optimisation_sphere_d&
          get_sphere (Homogeneous_tag t) const
        {return *this;}

        Optimisation_sphere_d (const Traits& t = Traits())
            : d(-1), m(0), s(0), tco(t)
        {}
        
        void set_tco (const Traits& tcobj)
        {
            tco = tcobj;
        }
        
        
        void init (int ambient_dimension)
        {
            d = ambient_dimension;
            m = 0;
            s = 0;
            sqr_r = -RT(1);
        
            q =             new RT*[d+1];
            inv =           new RT**[d+1];
            denom =         new RT[d+1];
            v_basis =       new RT*[d+1];
            x =             new RT[d+2];
            v =             new RT[d+2];
            c =             new RT[d+1];
        
            for (int j=0; j<d+1; ++j) {
                q[j] =      new RT[d];
                inv[j] =    new RT*[j+2];
                v_basis[j] =new RT[j+2];
                for (int row=0; row<j+2; ++row)
                    inv[j][row] = new RT[row+1];
            }
        
            for (int i=0; i<d; ++i)
                c[i] = RT(0);
            c[d] = RT(1);
        }
        
        
        ~Optimisation_sphere_d ()
        {
            if (d != -1)
                destroy();
        }
        
        
        void destroy ()
        {
            for (int j=0; j<d+1; ++j) {
                for (int row=0; row<j+2; ++row)
                    delete[] inv[j][row];
                delete[] v_basis[j];
                delete[] inv[j];
                delete[] q[j];
            }
            delete[] c;
            delete[] v;
            delete[] x;
            delete[] v_basis;
            delete[] denom;
            delete[] inv;
            delete[] q;
        }
        
        
        void set_size (int ambient_dimension)
        {
            if (d != -1)
                destroy();
            if (ambient_dimension != -1)
                init(ambient_dimension);
            else {
                d = -1;
                m = 0;
                s = 0;
            }
        }
        
        
        void push (const PT& p)
        {
            // store q_m = p by copying its cartesian part into q[m]
            It i(tco.access_coordinates_begin_d_object()(p)); RT *o;
            for (o=q[m]; o<q[m]+d; *(o++)=*(i++));
        
            // get homogenizing coordinate
            RT hom = *(i++);
        
            if (m==0)
            {
                // set up v_{B^0} directly
                v_basis[0][0] = hom;
                v_basis[0][1] = prod(q[0],q[0],d);
        
                // set up \tilde{A}^{-1}_{B^0} directly
                RT** M = inv[0];
                M[0][0] = RT_(2)*v_basis[0][1];
                M[1][0] = -hom;
                M[1][1] = RT_(0);
                denom[0] = -CGAL_NTS square(hom);  // det(\tilde{A}_{B^0})
        
            } else {
                // set up v_{B^m}
                int j;
                RT sqr_q_m = prod(q[m],q[m],d);
                v_basis[m][m+1] = v_basis[m-1][0]*sqr_q_m;
                for (j=0; j<m+1; ++j)
                    v_basis[m][j] = hom*v_basis[m-1][j];
        
        
                // set up vector v by computing 2q_j^T q_m, j=0,...,m-1
                v[0] = hom;
                for (j=0; j<m; ++j)
                    v[j+1] = RT_(2)*prod(q[j],q[m],d);
        
                // compute \tilde{a}_0,...,\tilde{a}_m
                multiply (m-1, v, x);               // x[j]=\tilde{a}_j, j=0,...,m
        
                // compute \tilde{z}
                RT old_denom = denom[m-1];
                RT z = old_denom*RT_(2)*sqr_q_m - prod(v,x,m+1);
                CGAL_optimisation_assertion (!CGAL_NTS is_zero (z));
        
                // set up \tilde{A}^{-1}_{B^m}
                RT** M = inv[m-1];          // \tilde{A}^{-1}_B, old matrix
                RT** M_new = inv[m];        // \tilde{A}^{-1}_{B'}, new matrix
        
                // first m rows
                int row, col;
                for (row=0; row<m+1; ++row)
                    for (col=0; col<row+1; ++col)
                        M_new [row][col]
                            = (z*M[row][col] + x[row]*x[col])/old_denom;
        
                // last row
                for (col=0; col<m+1; ++col)
                    M_new [m+1][col] = -x[col];
                M_new [m+1][m+1] = old_denom;
        
                // new denominator
                denom[m] = z;
            }
            s = ++m;
            compute_c_and_sqr_r();
        }
        
        
        void pop ()
        {
            --m;
        }
        
        
        RT excess (const PT& p) const
        {
            // store hD times the cartesian part of p in v
            RT hD = c[d];
            It i(tco.access_coordinates_begin_d_object()(p)); RT *o;
        
                for ( o=v; o<v+d; *(o++)=hD*(*(i++)));
        
                // get h_p
                RT h_p = *(i++);
                CGAL_optimisation_precondition (!CGAL_NTS is_zero (h_p));
        
                // compute (h_p h D)^2 (c-p)^2
                RT sqr_dist(RT(0));
                for (int k=0; k<d; ++k)
                    sqr_dist += CGAL_NTS square(h_p*c[k]-v[k]);
        
                // compute excess
                return sqr_dist - CGAL_NTS square(h_p)*sqr_r;
             }
        
        
        
        PT center () const
        {
             return tco.construct_point_d_object()(d,c,c+d+1);
        }
        
        FT squared_radius () const
        {
             return FT(sqr_r)/FT(CGAL_NTS square(c[d]));
        }
        
        int number_of_support_points () const
        {
             return s;
        }
        
        int size_of_basis () const
        {
             return m;
        }
        
        
        bool is_valid (bool verbose = false, int level = true) const
        {
            if (d==-1) return true;
            Verbose_ostream verr (verbose);
            int sign_hD = CGAL::sign(c[d]), s_old = 1,
                s_new = CGAL::sign(v_basis[0][0]), signum;
            for (int j=1; j<m+1; ++j) {
                signum = sign_hD * s_old * s_new * CGAL::sign(x[j]);
                if (!CGAL_NTS is_positive (signum))
                    return (_optimisation_is_valid_fail
                        (verr, "center not in convex hull of support points"));
                s_old = s_new; s_new = CGAL::sign(v_basis[j][0]);
            }
            return true;
        }
        
        
    private:
        void multiply (int j, const RT* vec, RT* res)
        {
            RT** M = inv[j];
            for (int row=0; row<j+2; ++row) {
                res[row] = prod(M[row],vec,row+1);
                for (int col = row+1; col<j+2; ++col)
                    res[row] += M[col][row]*vec[col];
            }
        }
        
        
        void compute_c_and_sqr_r ()
        {
            // solve
            multiply (m-1, v_basis[m-1], x);
        
            // set cartesian part
            for (int i=0; i<d; ++i) c[i] = RT(0);
                for (int j=0; j<m; ++j) {
                    RT l = x[j+1], *q_j = q[j];
                    for (int i=0; i<d; ++i)
                        c[i] += l*q_j[i];
            }
            c[d] = v_basis[m-1][0]*denom[m-1];                // hD
            sqr_r = x[0]*c[d] + prod(c,c,d); // \tilde{\alpha}hD+c^Tc
        }
        
        
        RT prod (const RT* v1, const RT* v2, int k) const
        {
            RT res = RT(0);
            for (const RT *i=v1, *j=v2; i<v1+k; res += (*(i++))*(*(j++)));
            return res;
        }
        
        
    };

     CGAL_END_NAMESPACE

    #endif // CGAL_OPTIMISATION_SPHERE_D_H



    // ===== EOF ==================================================================