wxMatrix2D wxMatrix2D::Mult(const wxMatrix2D &B) const
{
    wxMatrix2D temp;
    wxCHECK_MSG(Ok() && B.Ok(), temp, wxT("Invalid matrix"));
    wxCHECK_MSG(GetWidth() == B.GetHeight(), temp, wxT("Invalid matrix dimensions for mult"));

    int b_width = B.GetWidth();
    int i, j, k, width = M_MATRIXDATA->m_width, height = M_MATRIXDATA->m_height;

    if (!temp.Create(b_width, height, true))
        return temp;

    double *data      = GetData();
    double *b_data    = B.GetData();
    double *temp_data = temp.GetData();

    for (j = 0; j < height; ++j)
    {
        for (i = 0; i < b_width; ++i)
        {
            for (k = 0; k < width; ++k)
            {
                temp_data[j+i*b_width] += data[k+i*width]*b_data[j+k*b_width];
            }
        }
    }

    return temp;
}

bool wxMatrix2D::IsEqual(const wxMatrix2D& matrix) const
{
    if (!Ok() || !matrix.Ok()) return false;
    if (GetSize() != matrix.GetSize()) return false;

    return memcmp(M_MATRIXDATA->m_data, matrix.GetData(),
                  M_MATRIXDATA->m_width*M_MATRIXDATA->m_height*sizeof(double)) == 0;
}

wxMatrix2D wxMatrix2D::InsertCols(int col, const wxMatrix2D& matrix) const
{
    wxMatrix2D temp;
    wxCHECK_MSG(Ok() && matrix.Ok(), temp, wxT("Invalid wxMatrix2D"));

    int j, width = M_MATRIXDATA->m_width, height = M_MATRIXDATA->m_height;

    wxCHECK_MSG(height == matrix.GetHeight(), temp, wxT("Number of rows must match in InsertRows"));
    wxCHECK_MSG((col >= -1) && (col <= width), temp, wxT("Invalid wxMatrix2D"));

    if (!temp.Create(width + matrix.GetWidth(), height, false))
        return temp;

    if (col < 0) col = width; // fix col to be positive, from 0 - width

    int matrix_width = matrix.GetWidth();
    int temp_width   = temp.GetWidth();
    double *data        = GetData();
    double *matrix_data = matrix.GetData();
    double *temp_data   = temp.GetData();

    for (j = 0; j < height; j++)
    {
        if (col < 1)                // prepend the matrix to the beginning
        {
            memcpy(&temp_data[j*temp_width], &matrix_data[j*matrix_width], sizeof(double)*matrix_width);
            memcpy(&temp_data[j*temp_width+matrix_width], &data[j*width], sizeof(double)*width);
        }
        else if (col >= width)     // append the matrix to the end
        {
            memcpy(&temp_data[j*temp_width], &data[j*width], sizeof(double)*width);
            memcpy(&temp_data[j*temp_width+width], &matrix_data[j*matrix_width], sizeof(double)*matrix_width);
        }
        else                        // insert the matrix in the middle
        {
            memcpy(&temp_data[j*temp_width], &data[j*width], sizeof(double)*col);
            memcpy(&temp_data[j*temp_width + col], &matrix_data[j*matrix_width], sizeof(double)*matrix_width);
            memcpy(&temp_data[j*temp_width + col + matrix_width], &data[j*width+col], sizeof(double)*(width-col));
        }
    }

    return temp;
}

bool wxMatrix2D::Create(const wxMatrix2D &source, bool full_copy)
{
    wxCHECK_MSG( source.Ok() && (&source != this), false, wxT("Invalid matrix") );

    UnRef();
    if (full_copy)
        Copy(source);
    else
        Ref(source);

    return Ok();
}