// Aqsis
// Copyright (c) 1997 - 2001, Paul C. Gregory
//
// Contact: pgregory@aqsis.com
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

/**
 * Copyright (C) 2006-2007  NTT DATA CORPORATION
 * 
 * Version: 1.0.0 2007/04/01
 *  
 */
package net.cellcomputing.himawari.library.types;

import net.cellcomputing.himawari.accessory.STLVector;


/**
 * 
 * ３次スプライン曲線を提供するクラス
 * 
 * @author NTT DATA Corporation
 */
public strictfp class CqSplineCubic {

	
    protected CqMatrix	m_matBasis = new CqMatrix();	///< Basis matrix.
    protected int		m_Step;							///< Evaluation window step size.
    protected int		m_cu;							///< Number of control points.
    protected STLVector<CqVector4D>	m_aControlPoints = new STLVector<CqVector4D>(CqVector4D.class);	///< Array of 4D control points.

    protected CqVector4D	m_vecFDPoint  = new CqVector4D();	///< Forward difference evaluation parameters.
    protected CqVector4D	m_vecFDDelta  = new CqVector4D();	///< Forward difference evaluation parameters.
    protected CqVector4D	m_vecFDDelta2 = new CqVector4D();	///< Forward difference evaluation parameters.
    protected CqVector4D	m_vecFDDelta3 = new CqVector4D();	///< Forward difference evaluation parameters.
    
    private static float[][]	gBezierBasis = {
    		{ -1.0f,       3.0f,      -3.0f,       1.0f},
    		{  3.0f,      -6.0f,       3.0f,       0.0f},
    		{ -3.0f,       3.0f,       0.0f,       0.0f},
    		{  1.0f,       0.0f,       0.0f,       0.0f}
    		};
    private static float[][]	gBSplineBasis	= {
    		{ -1.0f/6.0f,  0.5f,      -0.5f,       1.0f/6.0f},
    		{  0.5f,      -1.0f,       0.5f,       0.0f},
    		{ -0.5f,       0.0f,	   0.5f,       0.0f},
    		{  1.0f/6.0f,  2.0f/3.0f,  1.0f/6.0f,  0.0f}
    		};
    private static float[][]	gCatmullRomBasis	= {
    		{ -0.5f,       1.5f,      -1.5f,       0.5f},
    		{  1.0f,      -2.5f,       2.0f,      -0.5f},
    		{ -0.5f,       0.0f,       0.5f,       0.0f},
    		{  0.0f,       1.0f,       0.0f,       0.0f}
    		};
    private static float[][]	gHermiteBasis	= {
    		{  2.0f,       1.0f,      -2.0f,       1.0f},
    		{ -3.0f,      -2.0f,       3.0f,      -1.0f},
    		{  0.0f,       1.0f,       0.0f,       0.0f},
    		{  1.0f,       0.0f,       0.0f,       0.0f}
    		};
    private static float[][]	gPowerBasis	= {
    		{  1.0f,       0.0f,       0.0f,       0.0f},
    		{  0.0f,       1.0f,       0.0f,       0.0f},
    		{  0.0f,       0.0f,       1.0f,       0.0f},
    		{  0.0f,       0.0f,       0.0f,       1.0f}
    		};
    
    
	//************************************************************************//
	//*** コンストラクタの定義
	//************************************************************************//
    
    public CqSplineCubic(){
    	m_matBasis = new CqMatrix( gBezierBasis );
    	m_aControlPoints.setSize( 4 );
    	m_cu = 4;
    	m_Step = 3;
    }
    
    public CqSplineCubic( int cu )
    {
    	m_matBasis = new CqMatrix( gBezierBasis );
        m_aControlPoints.setSize( cu );
        m_cu = cu;
        m_Step = 3;
    }
    
    
	//************************************************************************//
	//*** メソッドの定義
	//************************************************************************//
    
    /** Evaluate a cubic spline curve at the specified time.
     */
    public CqVector4D Evaluate( float t )
    {
        // Set up the geometry vector.
        CqVector4D	Gx = new CqVector4D();
        CqVector4D	Gy = new CqVector4D();
        CqVector4D	Gz = new CqVector4D();

        float u = (float)( cSections() ) * t;
        int iSection = (int)( u );
        t = u - iSection;
        int iv = iSection * m_Step;

        Gx.x = m_aControlPoints.get( 0 + iv ).x;
        Gx.y = m_aControlPoints.get( 1 + iv ).x;
        Gx.z = m_aControlPoints.get( 2 + iv ).x;
        Gx.w = m_aControlPoints.get( 3 + iv ).x;

        Gy.x = m_aControlPoints.get( 0 + iv ).y;
        Gy.y = m_aControlPoints.get( 1 + iv ).y;
        Gy.z = m_aControlPoints.get( 2 + iv ).y;
        Gy.w = m_aControlPoints.get( 3 + iv ).y;

        Gz.x = m_aControlPoints.get( 0 + iv ).z;
        Gz.y = m_aControlPoints.get( 1 + iv ).z;
        Gz.z = m_aControlPoints.get( 2 + iv ).z;
        Gz.w = m_aControlPoints.get( 3 + iv ).z;

//        Gx = Gx.mul( m_matBasis );
//        Gy = Gy.mul( m_matBasis );
//        Gz = Gz.mul( m_matBasis );
        Gx = m_matBasis.PreMultiply( Gx );
        Gy = m_matBasis.PreMultiply( Gy );
        Gz = m_matBasis.PreMultiply( Gz );

        float t2 = t * t;
        float t3 = t2 * t;

        float x = t3 * Gx.x + t2 * Gx.y + t * Gx.z + Gx.w;
        float y = t3 * Gy.x + t2 * Gy.y + t * Gy.z + Gy.w;
        float z = t3 * Gz.x + t2 * Gz.y + t * Gz.z + Gz.w;

        return new CqVector4D( x, y, z, 1 );
    }
    
    /** Intialise the forward differencing variables.
     */
    public void InitFD( int n )
    {
        float	d = 1.0f / (float)( n );
        float	d2 = d * d;
        float	d3 = d2 * d;

        // Calculate the deltas.
        CqVector4D	Cx = new CqVector4D();
        CqVector4D	Cy = new CqVector4D();
        CqVector4D	Cz = new CqVector4D();

        Cx.x = m_aControlPoints.get( 0 ).x;
        Cx.y = m_aControlPoints.get( 1 ).x;
        Cx.z = m_aControlPoints.get( 2 ).x;
        Cx.w = m_aControlPoints.get( 3 ).x;

        Cy.x = m_aControlPoints.get( 0 ).y;
        Cy.y = m_aControlPoints.get( 1 ).y;
        Cy.z = m_aControlPoints.get( 2 ).y;
        Cy.w = m_aControlPoints.get( 3 ).y;

        Cz.x = m_aControlPoints.get( 0 ).z;
        Cz.y = m_aControlPoints.get( 1 ).z;
        Cz.z = m_aControlPoints.get( 2 ).z;
        Cz.w = m_aControlPoints.get( 3 ).z;

//        Cx = m_matBasis * Cx;
//        Cy = m_matBasis * Cy;
//        Cz = m_matBasis * Cz;
        Cx = m_matBasis.multiply( Cx );
        Cy = m_matBasis.multiply( Cy );
        Cz = m_matBasis.multiply( Cz );

        // Thisis basically an optimised version of the matrix multiply of
        //						[0   ,0   ,0,1][a]
        //						[d3  ,d2  ,d,0][b]
        //						[6*d3,2*d2,0,0][c]
        //						[6*d3,0   ,0,0][d]
        Cx.z = Cx.z * d + Cx.y * d2 + Cx.x * d3;
        Cx.x = Cx.x * 6 * d3;
        Cx.y = Cx.y * 2 * d2 + Cx.x;

        Cy.z = Cy.z * d + Cy.y * d2 + Cy.x * d3;
        Cy.x = Cy.x * 6 * d3;
        Cy.y = Cy.y * 2 * d2 + Cy.x;

        Cz.z = Cz.z * d + Cz.y * d2 + Cz.x * d3;
        Cz.x = Cz.x * 6 * d3;
        Cz.y = Cz.y * 2 * d2 + Cz.x;

        m_vecFDPoint	= new CqVector4D( Cx.w, Cy.w, Cz.w, 1 );
        m_vecFDDelta	= new CqVector4D( Cx.z, Cy.z, Cz.z, 1 );
        m_vecFDDelta2	= new CqVector4D( Cx.y, Cy.y, Cz.y, 1 );
        m_vecFDDelta3	= new CqVector4D( Cx.x, Cy.x, Cz.x, 1 );
    }
    
    /** Evaluate the curve using forward differencing.
     */
    public CqVector4D EvaluateFD()
    {
        CqVector4D	vecPoint = m_vecFDPoint;

        m_vecFDPoint .assignAdd( m_vecFDDelta );
        m_vecFDDelta .assignAdd( m_vecFDDelta2 );
        m_vecFDDelta2.assignAdd( m_vecFDDelta3 );

        return ( vecPoint );
    }

    /** Get a reference to the array of control points.
     */
    public STLVector<CqVector4D> aControlPoints()
    {
//    	return ( new STLVector<CqVector4D>(m_aControlPoints) );
    	STLVector<CqVector4D> tmp = new STLVector<CqVector4D>( CqVector4D.class, m_aControlPoints.size() );
    	for( int i=0; i<m_aControlPoints.size(); i++ ){
    		tmp.get(i).assignment( m_aControlPoints.get(i) );
    	}
    	return tmp;
    }
    
    /** Get a reference to the cubic spline basis matrix.
     */
    public CqMatrix matBasis()
    {
    	return ( new CqMatrix(m_matBasis) );
    }
    
    /** Set the cubic spline basis matrix.
     * @param mat Basis matrix.
     */
    public void SetmatBasis( CqMatrix mat )
    {
        m_matBasis.assignment( mat );
    }
    
    /** Set the cubic spline basis matrix.
     * @param strName Basis name.
     */
    public void SetBasis( final String strName )
    {
    	float[][] pVals = null;
		int step = 3;
        if ( strName.compareTo( "bezier" ) == 0 )
		{
            pVals = gBezierBasis;
			step = 3;
		}
        else if ( strName.compareTo( "bspline" ) == 0 )
		{
            pVals = gBSplineBasis;
			step = 1;
		}
        else if ( strName.compareTo( "catmull-rom" ) == 0 )
		{
            pVals = gCatmullRomBasis;
			step = 1;
		}
        else if ( strName.compareTo( "hermite" ) == 0 )
		{
            pVals = gHermiteBasis;
			step = 2;
		}
        else if ( strName.compareTo( "power" ) == 0 )
		{
            pVals = gPowerBasis;
			step =4;
		}

        if ( pVals!=null )
        {
//            CqMatrix m;
//            m = pVals;
        	CqMatrix m = new CqMatrix( pVals );
            SetmatBasis( m );
			SetStep( step );
        }
    }

    /** Get the control point step size for the evaluation window.
     * @return Integer step size.
     */
    public int Step()
    {
        return ( m_Step );
    }
    
    /** Set the control point step size for the evaluation window.
     * @param Step Integer step size.
     */
    public void SetStep( int Step )
    {
        m_Step = Step;
    }
    
    /** 
     * Return the number of curve sections in the spline curve
     * 
     * @return 
     */
    public int cSections()
    {
        return ( ( ( m_cu -4 ) / m_Step ) + 1 );
    }
    
    
    
	//************************************************************************//
	//*** 演算子のオーバーロード
	//************************************************************************//
    
    /** Indexed access to the control points.
     * @param i Integer index.
     */
    public CqVector4D valueAt( int i )
    {
        assert( i<m_cu );
        return m_aControlPoints.get(i);
    }
    
//    public void setValueAt( int i, CqVector4D value )
//    {
//    	assert( i<m_cu );
//    	m_aControlPoints.set( i, value );
//    }
    
}
