blasfeo-0.1.2/auxiliary/s_aux_lib4.c

/**************************************************************************************************
*                                                                                                 *
* This file is part of BLASFEO.                                                                   *
*                                                                                                 *
* BLASFEO -- BLAS For Embedded Optimization.                                                      *
* Copyright (C) 2019 by Gianluca Frison.                                                          *
* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
* All rights reserved.                                                                            *
*                                                                                                 *
* The 2-Clause BSD License                                                                        *
*                                                                                                 *
* Redistribution and use in source and binary forms, with or without                              *
* modification, are permitted provided that the following conditions are met:                     *
*                                                                                                 *
* 1. Redistributions of source code must retain the above copyright notice, this                  *
*    list of conditions and the following disclaimer.                                             *
* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
*    this list of conditions and the following disclaimer in the documentation                    *
*    and/or other materials provided with the distribution.                                       *
*                                                                                                 *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
*                                                                                                 *
* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
*                                                                                                 *
**************************************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "../include/blasfeo_common.h"
#include "../include/blasfeo_block_size.h"
#include "../include/blasfeo_s_kernel.h"


// scales and adds a strvec into a strvec
void blasfeo_svecad(int m, float *alphap, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci)
	{
	float alpha = alphap[0];
	float *pa = sa->pa + ai;
	float *pc = sc->pa + ci;
	int ii;
	ii = 0;
	for(; ii<m-3; ii+=4)
		{
		pc[ii+0] += alpha*pa[ii+0];
		pc[ii+1] += alpha*pa[ii+1];
		pc[ii+2] += alpha*pa[ii+2];
		pc[ii+3] += alpha*pa[ii+3];
		}
	for(; ii<m; ii++)
		{
		pc[ii+0] += alpha*pa[ii+0];
		}
	return;
	}


// transpose general matrix; m and n are referred to the original matrix
void sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc)
	{

/*

m = 5
n = 3
offsetA = 1
offsetC = 2

A =
 x x x
 -
 x x x
 x x x
 x x x
 x x x

C =
 x x x x x
 x x x x x
 -
 x x x x x

*/

	if(m<=0 || n<=0)
		return;

	const int bs = 4;

	int mna = (bs-offsetA%bs)%bs;
	mna = m<mna ? m : mna;
	int nna = (bs-offsetC%bs)%bs;
	nna = n<nna ? n : nna;

	int ii;

	ii = 0;

	if(mna>0)
		{
		if(mna==1)
			kernel_sgetr_1_lib4(0, n, nna, alpha, pA, pC, sdc);
		else if(mna==2)
			kernel_sgetr_2_lib4(0, n, nna, alpha, pA, pC, sdc);
		else //if(mna==3)
			kernel_sgetr_3_lib4(0, n, nna, alpha, pA, pC, sdc);
		ii += mna;
		pA += mna + bs*(sda-1);
		pC += mna*bs;
		}
	for( ; ii<m-3; ii+=4)
//	for( ; ii<m; ii+=4)
		{
		kernel_sgetr_4_lib4(0, n, nna, alpha, pA, pC, sdc);
		pA += bs*sda;
		pC += bs*bs;
		}

	// clean-up at the end using smaller kernels
	if(ii==m)
		return;

	if(m-ii==1)
		kernel_sgetr_1_lib4(0, n, nna, alpha, pA, pC, sdc);
	else if(m-ii==2)
		kernel_sgetr_2_lib4(0, n, nna, alpha, pA, pC, sdc);
	else if(m-ii==3)
		kernel_sgetr_3_lib4(0, n, nna, alpha, pA, pC, sdc);

	return;

	}


// transpose lower triangular matrix
void strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc)
	{

/*

A =
 x
 x x
 x x x
 x x x x

 x x x x x
 x x x x x x
 x x x x x x x
 x x x x x x x x

C =
 x x x x x x x x

   x x x x x x x
     x x x x x x
	   x x x x x
	     x x x x

	       x x x
	         x x
	           x

*/

	int n = m;

	if(m<=0 || n<=0)
		return;

	const int bs = 4;

	int mna = (bs-offsetA%bs)%bs;
	mna = m<mna ? m : mna;
	int nna = (bs-offsetC%bs)%bs;
	nna = n<nna ? n : nna;

	int ii;

	ii = 0;

	if(mna>0)
		{
		if(mna==1)
			{
			pC[0] = alpha * pA[0];
			}
		else if(mna==2)
			{
			if(nna==1)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[1+bs*(0+sdc)] = alpha * pA[1+bs*1];
				}
			else
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				}
			}
		else //if(mna==3)
			{
			if(nna==1)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[0+bs*2] = alpha * pA[2+bs*0];
				pC[1+bs*(0+sdc)] = alpha * pA[1+bs*1];
				pC[1+bs*(1+sdc)] = alpha * pA[2+bs*1];
				pC[2+bs*(1+sdc)] = alpha * pA[2+bs*2];
				}
			else if(nna==2)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[0+bs*2] = alpha * pA[2+bs*0];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pC[1+bs*2] = alpha * pA[2+bs*1];
				pC[2+bs*(1+sdc)] = alpha * pA[2+bs*2];
				}
			else
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[0+bs*2] = alpha * pA[2+bs*0];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pC[1+bs*2] = alpha * pA[2+bs*1];
				pC[2+bs*2] = alpha * pA[2+bs*2];
				}
			}
		ii += mna;
		pA += mna + bs*(sda-1);
		pC += mna*bs;
		}
	for( ; ii<m-3; ii+=4)
		{
		kernel_sgetr_4_lib4(1, ii, nna, alpha, pA, pC, sdc);
		pA += bs*sda;
		pC += bs*bs;
		}

	// clean-up at the end using smaller kernels
	if(ii==m)
		return;

	if(m-ii==1)
		kernel_sgetr_1_lib4(1, ii, nna, alpha, pA, pC, sdc);
	else if(m-ii==2)
		kernel_sgetr_2_lib4(1, ii, nna, alpha, pA, pC, sdc);
	else if(m-ii==3)
		kernel_sgetr_3_lib4(1, ii, nna, alpha, pA, pC, sdc);

	return;

	}


// transpose an aligned upper triangular matrix into an aligned lower triangular matrix
void strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc)
	{

/*

A =
 x x x x x x x x
   x x x x x x x

     x x x x x x
       x x x x x
         x x x x
           x x x
             x x
               x

C =
 x

 x x
 x x x
 x x x x
 x x x x x
 x x x x x x
 x x x x x x x
 x x x x x x x x

*/

	int n = m;

	if(m<=0 || n<=0)
		return;

	const int bs = 4;

	int mna = (bs-offsetA%bs)%bs;
	mna = m<mna ? m : mna;
	int nna = (bs-offsetC%bs)%bs;
	nna = n<nna ? n : nna;
	int tna = nna;

	int ii;

	ii = 0;

	if(mna>0)
		{
		if(mna==1)
			{
			kernel_sgetr_1_lib4(0, n, nna, alpha, pA, pC, sdc);
			if(nna!=1)
				{
//				pC[0+bs*0] = alpha * pA[0+bs*0];
				pA += 1*bs;
				pC += 1;
				tna = (bs-(offsetC+1)%bs)%bs;
				}
			else //if(nna==1)
				{
//				pC[0+bs*0] = alpha * pA[0+bs*0];
				pA += 1*bs;
				pC += 1 + (sdc-1)*bs;
				tna = 0; //(bs-(offsetC+1)%bs)%bs;
				}
//			kernel_sgetr_1_lib4(0, n-1, tna, alpha, pA, pC, sdc);
			}
		else if(mna==2)
			{
			if(nna==0 || nna==3)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pA += 2*bs;
				pC += 2;
				tna = (bs-(offsetC+2)%bs)%bs;
				kernel_sgetr_2_lib4(0, n-2, tna, alpha, pA, pC, sdc);
				}
			else if(nna==1)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pA += 1*bs;
				pC += 1 + (sdc-1)*bs;
//				pC[0+bs*0] = alpha * pA[0+bs*0];
//				pC[0+bs*1] = alpha * pA[1+bs*0];
				kernel_sgetr_2_lib4(0, n-1, 0, alpha, pA, pC, sdc);
				pA += 1*bs;
				pC += 1;
				tna = 3; //(bs-(offsetC+2)%bs)%bs;
//				kernel_sgetr_2_lib4(0, n-2, tna, alpha, pA, pC, sdc);
				}
			else if(nna==2)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pA += 2*bs;
				pC += 2 + (sdc-1)*bs;
				tna = 0; //(bs-(offsetC+2)%bs)%bs;
				kernel_sgetr_2_lib4(0, n-2, tna, alpha, pA, pC, sdc);
				}
			}
		else //if(mna==3)
			{
			if(nna==0)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pC[2+bs*0] = alpha * pA[0+bs*2];
				pC[2+bs*1] = alpha * pA[1+bs*2];
				pC[2+bs*2] = alpha * pA[2+bs*2];
				pA += 3*bs;
				pC += 3;
				tna = 1;
				kernel_sgetr_3_lib4(0, n-3, tna, alpha, pA, pC, sdc);
				}
			else if(nna==1)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pA += bs;
				pC += 1 + (sdc-1)*bs;
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[0+bs*1] = alpha * pA[1+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pC[1+bs*2] = alpha * pA[2+bs*1];
				pA += 2*bs;
				pC += 2;
				tna = 2;
				kernel_sgetr_3_lib4(0, n-3, tna, alpha, pA, pC, sdc);
				}
			else if(nna==2)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pA += 2*bs;
				pC += 2 + (sdc-1)*bs;
//				pC[0+bs*0] = alpha * pA[0+bs*0];
//				pC[0+bs*1] = alpha * pA[1+bs*0];
//				pC[0+bs*2] = alpha * pA[2+bs*0];
				kernel_sgetr_3_lib4(0, n-2, 0, alpha, pA, pC, sdc);
				pA += 1*bs;
				pC += 1;
				tna = 3;
//				kernel_sgetr_3_lib4(0, n-3, tna, alpha, pA, pC, sdc);
				}
			else //if(nna==3)
				{
				pC[0+bs*0] = alpha * pA[0+bs*0];
				pC[1+bs*0] = alpha * pA[0+bs*1];
				pC[1+bs*1] = alpha * pA[1+bs*1];
				pC[2+bs*0] = alpha * pA[0+bs*2];
				pC[2+bs*1] = alpha * pA[1+bs*2];
				pC[2+bs*2] = alpha * pA[2+bs*2];
				pA += 3*bs;
				pC += 3 + (sdc-1)*bs;
				tna = 0;
				kernel_sgetr_3_lib4(0, n-3, tna, alpha, pA, pC, sdc);
				}
			}
		ii += mna;
		pA += mna + bs*(sda-1);
		pC += mna*bs;
		}
	for( ; ii<m-3; ii+=4)
		{
		if(tna==0)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[2+bs*0] = alpha * pA[0+bs*2];
			pC[2+bs*1] = alpha * pA[1+bs*2];
			pC[2+bs*2] = alpha * pA[2+bs*2];
			pC[3+bs*0] = alpha * pA[0+bs*3];
			pC[3+bs*1] = alpha * pA[1+bs*3];
			pC[3+bs*2] = alpha * pA[2+bs*3];
			pC[3+bs*3] = alpha * pA[3+bs*3];
			pA += 4*bs;
			pC += sdc*bs;
			kernel_sgetr_4_lib4(0, n-ii-4, 0, alpha, pA, pC, sdc);
			}
		else if(tna==1)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pA += bs;
			pC += 1 + (sdc-1)*bs;
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[0+bs*1] = alpha * pA[1+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[1+bs*2] = alpha * pA[2+bs*1];
			pC[2+bs*0] = alpha * pA[0+bs*2];
			pC[2+bs*1] = alpha * pA[1+bs*2];
			pC[2+bs*2] = alpha * pA[2+bs*2];
			pC[2+bs*3] = alpha * pA[3+bs*2];
			pA += 3*bs;
			pC += 3;
			kernel_sgetr_4_lib4(0, n-ii-4, 1, alpha, pA, pC, sdc);
			}
		else if(tna==2)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pA += 2*bs;
			pC += 2 + (sdc-1)*bs;
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[0+bs*1] = alpha * pA[1+bs*0];
			pC[0+bs*2] = alpha * pA[2+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[1+bs*2] = alpha * pA[2+bs*1];
			pC[1+bs*3] = alpha * pA[3+bs*1];
			pA += 2*bs;
			pC += 2;
			kernel_sgetr_4_lib4(0, n-ii-4, 2, alpha, pA, pC, sdc);
			}
		else //if(tna==3)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[2+bs*0] = alpha * pA[0+bs*2];
			pC[2+bs*1] = alpha * pA[1+bs*2];
			pC[2+bs*2] = alpha * pA[2+bs*2];
			pA += 3*bs;
			pC += 3 + (sdc-1)*bs;
			kernel_sgetr_4_lib4(0, n-ii-3, 0, alpha, pA, pC, sdc);
//			pC[0+bs*0] = alpha * pA[0+bs*0];
//			pC[0+bs*1] = alpha * pA[1+bs*0];
//			pC[0+bs*2] = alpha * pA[2+bs*0];
//			pC[0+bs*3] = alpha * pA[3+bs*0];
			pA += bs;
			pC += 1;
//			kernel_sgetr_4_lib4(0, n-ii-4, tna, alpha, pA, pC, sdc);
			}
		pA += bs*sda;
		pC += bs*bs;
		}

	// clean-up at the end
	if(ii==m)
		return;

	if(m-ii==1)
		{
		pC[0+bs*0] = alpha * pA[0+bs*0];
		}
	else if(m-ii==2)
		{
		if(tna!=1)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			}
		else //if(tna==1)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pA += bs;
			pC += 1 + (sdc-1)*bs;
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[0+bs*1] = alpha * pA[1+bs*0];
			}
		}
	else if(m-ii==3)
		{
		if(tna==0 || tna==3)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[2+bs*0] = alpha * pA[0+bs*2];
			pC[2+bs*1] = alpha * pA[1+bs*2];
			pC[2+bs*2] = alpha * pA[2+bs*2];
			}
		else if(tna==1)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pA += bs;
			pC += 1 + (sdc-1)*bs;
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[0+bs*1] = alpha * pA[1+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pC[1+bs*2] = alpha * pA[2+bs*1];
			}
		else //if(tna==2)
			{
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[1+bs*0] = alpha * pA[0+bs*1];
			pC[1+bs*1] = alpha * pA[1+bs*1];
			pA += 2*bs;
			pC += 2 + (sdc-1)*bs;
			pC[0+bs*0] = alpha * pA[0+bs*0];
			pC[0+bs*1] = alpha * pA[1+bs*0];
			pC[0+bs*2] = alpha * pA[2+bs*0];
			}
		}

	return;

	}


// regularize diagonal
void sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll+bs*ll] += reg;
			}
		pD += kna + bs*(sdd-1) + kna*bs;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+(jj+0)*bs+0] += reg;
		pD[jj*sdd+(jj+1)*bs+1] += reg;
		pD[jj*sdd+(jj+2)*bs+2] += reg;
		pD[jj*sdd+(jj+3)*bs+3] += reg;
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+(jj+ll)*bs+ll] += reg;
		}

	}


// insert vector to diagonal
void sdiain_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll+bs*ll] = alpha*x[ll];
			}
		pD += kna + bs*(sdd-1) + kna*bs;
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+(jj+0)*bs+0] = alpha*x[jj+0];
		pD[jj*sdd+(jj+1)*bs+1] = alpha*x[jj+1];
		pD[jj*sdd+(jj+2)*bs+2] = alpha*x[jj+2];
		pD[jj*sdd+(jj+3)*bs+3] = alpha*x[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+(jj+ll)*bs+ll] = alpha*x[jj+ll];
		}

	}


// insert sqrt of vector to diagonal
void sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll+bs*ll] = sqrt(x[ll]);
			}
		pD += kna + bs*(sdd-1) + kna*bs;
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+(jj+0)*bs+0] = sqrt(x[jj+0]);
		pD[jj*sdd+(jj+1)*bs+1] = sqrt(x[jj+1]);
		pD[jj*sdd+(jj+2)*bs+2] = sqrt(x[jj+2]);
		pD[jj*sdd+(jj+3)*bs+3] = sqrt(x[jj+3]);
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+(jj+ll)*bs+ll] = sqrt(x[jj+ll]);
		}

	}


// extract diagonal to vector
void sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			x[ll] = alpha * pD[ll+bs*ll];
			}
		pD += kna + bs*(sdd-1) + kna*bs;
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		x[jj+0] = alpha * pD[jj*sdd+(jj+0)*bs+0];
		x[jj+1] = alpha * pD[jj*sdd+(jj+1)*bs+1];
		x[jj+2] = alpha * pD[jj*sdd+(jj+2)*bs+2];
		x[jj+3] = alpha * pD[jj*sdd+(jj+3)*bs+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		x[jj+ll] = alpha * pD[jj*sdd+(jj+ll)*bs+ll];
		}

	}


// add scaled vector to diagonal
void sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll+bs*ll] += alpha * x[ll];
			}
		pD += kna + bs*(sdd-1) + kna*bs;
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+(jj+0)*bs+0] += alpha * x[jj+0];
		pD[jj*sdd+(jj+1)*bs+1] += alpha * x[jj+1];
		pD[jj*sdd+(jj+2)*bs+2] += alpha * x[jj+2];
		pD[jj*sdd+(jj+3)*bs+3] += alpha * x[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+(jj+ll)*bs+ll] += alpha * x[jj+ll];
		}

	}


// insert vector to diagonal, sparse formulation
void sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii/bs*bs*sdd+ii%bs+ii*bs] = alpha * x[jj];
		}

	}


// extract diagonal to vector, sparse formulation
void sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		x[jj] = alpha * pD[ii/bs*bs*sdd+ii%bs+ii*bs];
		}

	}


// add scaled vector to diagonal, sparse formulation
void sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii/bs*bs*sdd+ii%bs+ii*bs] += alpha * x[jj];
		}

	}


// add scaled vector to another vector and insert to diagonal, sparse formulation
void sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii/bs*bs*sdd+ii%bs+ii*bs] = y[jj] + alpha * x[jj];
		}

	}


// insert vector to row
void srowin_lib(int kmax, float alpha, float *x, float *pD)
	{

	const int bs = 4;

	int jj, ll;

	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[(jj+0)*bs] = alpha*x[jj+0];
		pD[(jj+1)*bs] = alpha*x[jj+1];
		pD[(jj+2)*bs] = alpha*x[jj+2];
		pD[(jj+3)*bs] = alpha*x[jj+3];
		}
	for(; jj<kmax; jj++)
		{
		pD[(jj)*bs] = alpha*x[jj];
		}

	}


// extract row to vector
void srowex_lib(int kmax, float alpha, float *pD, float *x)
	{

	const int bs = 4;

	int jj, ll;

	for(jj=0; jj<kmax-3; jj+=4)
		{
		x[jj+0] = alpha*pD[(jj+0)*bs];
		x[jj+1] = alpha*pD[(jj+1)*bs];
		x[jj+2] = alpha*pD[(jj+2)*bs];
		x[jj+3] = alpha*pD[(jj+3)*bs];
		}
	for(; jj<kmax; jj++)
		{
		x[jj] = alpha*pD[(jj)*bs];
		}

	}


// add scaled vector to row
void srowad_lib(int kmax, float alpha, float *x, float *pD)
	{

	const int bs = 4;

	int jj, ll;

	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[(jj+0)*bs] += alpha * x[jj+0];
		pD[(jj+1)*bs] += alpha * x[jj+1];
		pD[(jj+2)*bs] += alpha * x[jj+2];
		pD[(jj+3)*bs] += alpha * x[jj+3];
		}
	for(; jj<kmax; jj++)
		{
		pD[(jj)*bs] += alpha * x[jj];
		}

	}


// insert vector to row, sparse formulation
void srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii*bs] = alpha*x[jj];
		}

	}


// add scaled vector to row, sparse formulation
void srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii*bs] += alpha * x[jj];
		}

	}


// add scaled vector to another vector and insert to row, sparse formulation
void srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii*bs] = y[jj] + alpha * x[jj];
		}

	}


// swap two rows
void srowsw_lib(int kmax, float *pA, float *pC)
	{

	const int bs = 4;

	int ii;
	float tmp;

	for(ii=0; ii<kmax-3; ii+=4)
		{
		tmp = pA[0+bs*0];
		pA[0+bs*0] = pC[0+bs*0];
		pC[0+bs*0] = tmp;
		tmp = pA[0+bs*1];
		pA[0+bs*1] = pC[0+bs*1];
		pC[0+bs*1] = tmp;
		tmp = pA[0+bs*2];
		pA[0+bs*2] = pC[0+bs*2];
		pC[0+bs*2] = tmp;
		tmp = pA[0+bs*3];
		pA[0+bs*3] = pC[0+bs*3];
		pC[0+bs*3] = tmp;
		pA += 4*bs;
		pC += 4*bs;
		}
	for( ; ii<kmax; ii++)
		{
		tmp = pA[0+bs*0];
		pA[0+bs*0] = pC[0+bs*0];
		pC[0+bs*0] = tmp;
		pA += 1*bs;
		pC += 1*bs;
		}

	}


// extract vector from column
void scolex_lib(int kmax, int offset, float *pD, int sdd, float *x)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			x[ll] = pD[ll];
			}
		pD += kna + bs*(sdd-1);
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		x[jj*sdd+0] = pD[jj+0];
		x[jj*sdd+1] = pD[jj+1];
		x[jj*sdd+2] = pD[jj+2];
		x[jj*sdd+3] = pD[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		x[jj*sdd+ll] = pD[jj+ll];
		}

	}


// insert vector to column
void scolin_lib(int kmax, float *x, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll] = x[ll];
			}
		pD += kna + bs*(sdd-1);
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+0] = x[jj+0];
		pD[jj*sdd+1] = x[jj+1];
		pD[jj*sdd+2] = x[jj+2];
		pD[jj*sdd+3] = x[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+ll] = x[jj+ll];
		}

	}


// add scaled vector to column
void scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd)
	{

	const int bs = 4;

	int kna = (bs-offset%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pD[ll] += alpha * x[ll];
			}
		pD += kna + bs*(sdd-1);
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pD[jj*sdd+0] += alpha * x[jj+0];
		pD[jj*sdd+1] += alpha * x[jj+1];
		pD[jj*sdd+2] += alpha * x[jj+2];
		pD[jj*sdd+3] += alpha * x[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pD[jj*sdd+ll] += alpha * x[jj+ll];
		}

	}


// insert vector to diagonal, sparse formulation
void scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii/bs*bs*sdd+ii%bs] = x[jj];
		}

	}


// add scaled vector to diagonal, sparse formulation
void scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd)
	{

	const int bs = 4;

	int ii, jj;

	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[ii/bs*bs*sdd+ii%bs] += alpha * x[jj];
		}

	}


// swaps two cols
void scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc)
	{

	const int bs = 4;

	int ii;

	float tmp;

	if(offsetA==offsetC)
		{
		if(offsetA>0)
			{
			ii = 0;
			for(; ii<bs-offsetA; ii++)
				{
				tmp = pA[0+bs*0];
				pA[0+bs*0] = pC[0+bs*0];
				pC[0+bs*0] = tmp;
				pA += 1;
				pC += 1;
				}
			pA += bs*(sda-1);
			pC += bs*(sdc-1);
			kmax -= bs-offsetA;
			}
		ii = 0;
		for(; ii<kmax-3; ii+=4)
			{
			tmp = pA[0+bs*0];
			pA[0+bs*0] = pC[0+bs*0];
			pC[0+bs*0] = tmp;
			tmp = pA[1+bs*0];
			pA[1+bs*0] = pC[1+bs*0];
			pC[1+bs*0] = tmp;
			tmp = pA[2+bs*0];
			pA[2+bs*0] = pC[2+bs*0];
			pC[2+bs*0] = tmp;
			tmp = pA[3+bs*0];
			pA[3+bs*0] = pC[3+bs*0];
			pC[3+bs*0] = tmp;
			pA += bs*sda;
			pC += bs*sdc;
			}
		for(; ii<kmax; ii++)
			{
			tmp = pA[0+bs*0];
			pA[0+bs*0] = pC[0+bs*0];
			pC[0+bs*0] = tmp;
			pA += 1;
			pC += 1;
			}
		}
	else
		{
		printf("\nscolsw: feature not implemented yet: offsetA!=offsetC\n\n");
		exit(1);
		}

	return;

	}


// insert vector to vector, sparse formulation
void svecin_libsp(int kmax, int *idx, float *x, float *y)
	{

	int jj;

	for(jj=0; jj<kmax; jj++)
		{
		y[idx[jj]] = x[jj];
		}

	}


// adds vector to vector, sparse formulation
void svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y)
	{

	int jj;

	for(jj=0; jj<kmax; jj++)
		{
		y[idx[jj]] += alpha * x[jj];
		}

	}


/****************************
* new interface
****************************/


#if defined(LA_HIGH_PERFORMANCE)


// return the memory size (in bytes) needed for a strmat
int blasfeo_memsize_smat(int m, int n)
	{
	const int bs = 4;
	int nc = S_NC;
	int al = bs*nc;
	int pm = (m+bs-1)/bs*bs;
	int cn = (n+nc-1)/nc*nc;
	int tmp = m<n ? (m+al-1)/al*al : (n+al-1)/al*al; // al(min(m,n)) // XXX max ???
	int memsize = (pm*cn+tmp)*sizeof(float);
	return memsize;
	}


int blasfeo_memsize_smat_ps(int ps, int m, int n)
	{
	int nc = S_NC;
	int al = ps*nc;
	int pm = (m+ps-1)/ps*ps;
	int cn = (n+nc-1)/nc*nc;
	int tmp = m<n ? (m+al-1)/al*al : (n+al-1)/al*al; // al(min(m,n)) // XXX max ???
	int memsize = (pm*cn+tmp)*sizeof(float);
	return memsize;
	}


// return the memory size (in bytes) needed for the digonal of a strmat
int blasfeo_memsize_diag_smat(int m, int n)
	{
	const int bs = 4;
	int nc = S_NC;
	int al = bs*nc;
	int tmp = m<n ? (m+al-1)/al*al : (n+al-1)/al*al; // al(min(m,n)) // XXX max ???
	int memsize = tmp*sizeof(float);
	return memsize;
	}


// create a matrix structure for a matrix of size m*n by using memory passed by a pointer
void blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int nc = S_NC;
	int al = bs*nc;
	sA->m = m;
	sA->n = n;
	int pm = (m+bs-1)/bs*bs;
	int cn = (n+nc-1)/nc*nc;
	sA->pm = pm;
	sA->cn = cn;
	float *ptr = (float *) memory;
	sA->pA = ptr;
	ptr += pm*cn;
	int tmp = m<n ? (m+al-1)/al*al : (n+al-1)/al*al; // al(min(m,n)) // XXX max ???
	sA->dA = ptr;
	ptr += tmp;
	sA->memsize = (pm*cn+tmp)*sizeof(float);
	return;
	}


void blasfeo_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	int nc = S_NC;
	int al = ps*nc;
	sA->m = m;
	sA->n = n;
	int pm = (m+ps-1)/ps*ps;
	int cn = (n+nc-1)/nc*nc;
	sA->pm = pm;
	sA->cn = cn;
	float *ptr = (float *) memory;
	sA->pA = ptr;
	ptr += pm*cn;
	int tmp = m<n ? (m+al-1)/al*al : (n+al-1)/al*al; // al(min(m,n)) // XXX max ???
	sA->dA = ptr;
	ptr += tmp;
	sA->memsize = (pm*cn+tmp)*sizeof(float);
	return;
	}


// return memory size (in bytes) needed for a strvec
int blasfeo_memsize_svec(int m)
	{
	const int bs = 4;
//	int nc = S_NC;
//	int al = bs*nc;
	int pm = (m+bs-1)/bs*bs;
	int memsize = pm*sizeof(float);
	return memsize;
	}


// create a vector structure for a vector of size m by using memory passed by a pointer
void blasfeo_create_svec(int m, struct blasfeo_svec *sa, void *memory)
	{
	const int bs = 4;
//	int nc = S_NC;
//	int al = bs*nc;
	sa->m = m;
	int pm = (m+bs-1)/bs*bs;
	sa->pm = pm;
	float *ptr = (float *) memory;
	sa->pa = ptr;
//	ptr += pm;
	sa->memsize = pm*sizeof(float);
	return;
	}


// convert a matrix into a matrix structure
void blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + aj*bs + ai/bs*bs*sda + ai%bs;
	int i, ii, j, jj, m0, m1, m2;
	float *B, *pB;
	m0 = (bs-ai%bs)%bs;
	if(m0>m)
		m0 = m;
	m1 = m - m0;
	jj = 0;
	for( ; jj<n-3; jj+=4)
		{
		B  =  A + jj*lda;
		pB = pA + jj*bs;
		ii = 0;
		if(m0>0)
			{
			for( ; ii<m0; ii++)
				{
				pB[ii+bs*0] = B[ii+lda*0];
				pB[ii+bs*1] = B[ii+lda*1];
				pB[ii+bs*2] = B[ii+lda*2];
				pB[ii+bs*3] = B[ii+lda*3];
				}
			B  += m0;
			pB += m0 + bs*(sda-1);
			}
		for( ; ii<m-3; ii+=4)
			{
			// col 0
			pB[0+bs*0] = B[0+lda*0];
			pB[1+bs*0] = B[1+lda*0];
			pB[2+bs*0] = B[2+lda*0];
			pB[3+bs*0] = B[3+lda*0];
			// col 1
			pB[0+bs*1] = B[0+lda*1];
			pB[1+bs*1] = B[1+lda*1];
			pB[2+bs*1] = B[2+lda*1];
			pB[3+bs*1] = B[3+lda*1];
			// col 2
			pB[0+bs*2] = B[0+lda*2];
			pB[1+bs*2] = B[1+lda*2];
			pB[2+bs*2] = B[2+lda*2];
			pB[3+bs*2] = B[3+lda*2];
			// col 3
			pB[0+bs*3] = B[0+lda*3];
			pB[1+bs*3] = B[1+lda*3];
			pB[2+bs*3] = B[2+lda*3];
			pB[3+bs*3] = B[3+lda*3];
			// update
			B  += 4;
			pB += bs*sda;
			}
		for( ; ii<m; ii++)
			{
			// col 0
			pB[0+bs*0] = B[0+lda*0];
			// col 1
			pB[0+bs*1] = B[0+lda*1];
			// col 2
			pB[0+bs*2] = B[0+lda*2];
			// col 3
			pB[0+bs*3] = B[0+lda*3];
			// update
			B  += 1;
			pB += 1;
			}
		}
	for( ; jj<n; jj++)
		{

		B  =  A + jj*lda;
		pB = pA + jj*bs;

		ii = 0;
		if(m0>0)
			{
			for( ; ii<m0; ii++)
				{
				pB[ii+bs*0] = B[ii+lda*0];
				}
			B  += m0;
			pB += m0 + bs*(sda-1);
			}
		for( ; ii<m-3; ii+=4)
			{
			// col 0
			pB[0+bs*0] = B[0+lda*0];
			pB[1+bs*0] = B[1+lda*0];
			pB[2+bs*0] = B[2+lda*0];
			pB[3+bs*0] = B[3+lda*0];
			// update
			B  += 4;
			pB += bs*sda;
			}
		for( ; ii<m; ii++)
			{
			// col 0
			pB[0+bs*0] = B[0+lda*0];
			// update
			B  += 1;
			pB += 1;
			}
		}
	return;
	}


// convert and transpose a matrix into a matrix structure
void blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + aj*bs + ai/bs*bs*sda + ai%bs;
	int i, ii, j, m0, m1, m2;
	float 	*B, *pB;
	m0 = (bs-ai%bs)%bs;
	if(m0>n)
		m0 = n;
	m1 = n - m0;
	ii = 0;
	if(m0>0)
		{
		for(j=0; j<m; j++)
			{
			for(i=0; i<m0; i++)
				{
				pA[i+j*bs+ii*sda] = A[j+(i+ii)*lda];
				}
			}
		A  += m0*lda;
		pA += m0 + bs*(sda-1);
		}
	ii = 0;
	for(; ii<m1-3; ii+=bs)
		{
		j=0;
		B  = A + ii*lda;
		pB = pA + ii*sda;
		for(; j<m-3; j+=4)
			{
			// unroll 0
			pB[0+0*bs] = B[0+0*lda];
			pB[1+0*bs] = B[0+1*lda];
			pB[2+0*bs] = B[0+2*lda];
			pB[3+0*bs] = B[0+3*lda];
			// unroll 1
			pB[0+1*bs] = B[1+0*lda];
			pB[1+1*bs] = B[1+1*lda];
			pB[2+1*bs] = B[1+2*lda];
			pB[3+1*bs] = B[1+3*lda];
			// unroll 2
			pB[0+2*bs] = B[2+0*lda];
			pB[1+2*bs] = B[2+1*lda];
			pB[2+2*bs] = B[2+2*lda];
			pB[3+2*bs] = B[2+3*lda];
			// unroll 3
			pB[0+3*bs] = B[3+0*lda];
			pB[1+3*bs] = B[3+1*lda];
			pB[2+3*bs] = B[3+2*lda];
			pB[3+3*bs] = B[3+3*lda];
			B  += 4;
			pB += 4*bs;
			}
		for(; j<m; j++)
			{
			// unroll 0
			pB[0+0*bs] = B[0+0*lda];
			pB[1+0*bs] = B[0+1*lda];
			pB[2+0*bs] = B[0+2*lda];
			pB[3+0*bs] = B[0+3*lda];
			B  += 1;
			pB += 1*bs;
			}
		}
	if(ii<m1)
		{
		m2 = m1-ii;
		if(bs<m2) m2 = bs;
		for(j=0; j<m; j++)
			{
			for(i=0; i<m2; i++)
				{
				pA[i+j*bs+ii*sda] = A[j+(i+ii)*lda];
				}
			}
		}
	return;
	}


// convert a vector into a vector structure
void blasfeo_pack_svec(int m, float *a, struct blasfeo_svec *sa, int ai)
	{
	float *pa = sa->pa + ai;
	int ii;
	for(ii=0; ii<m; ii++)
		pa[ii] = a[ii];
	return;
	}


// convert a matrix structure into a matrix
void blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda)
	{
	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + aj*bs + ai/bs*bs*sda + ai%bs;
	int i, ii, jj;
	int m0 = (bs-ai%bs)%bs;
	if(m0>m)
		m0 = m;
	float *ptr_pA;
	jj=0;
	for(; jj<n-3; jj+=4)
		{
		ptr_pA = pA + jj*bs;
		ii = 0;
		if(m0>0)
			{
			for(; ii<m0; ii++)
				{
				// unroll 0
				A[ii+lda*(jj+0)] = ptr_pA[0+bs*0];
				// unroll 1
				A[ii+lda*(jj+1)] = ptr_pA[0+bs*1];
				// unroll 2
				A[ii+lda*(jj+2)] = ptr_pA[0+bs*2];
				// unroll 3
				A[ii+lda*(jj+3)] = ptr_pA[0+bs*3];
				ptr_pA++;
				}
			ptr_pA += (sda-1)*bs;
			}
		for(; ii<m-bs+1; ii+=bs)
			{
			// unroll 0
			A[0+ii+lda*(jj+0)] = ptr_pA[0+bs*0];
			A[1+ii+lda*(jj+0)] = ptr_pA[1+bs*0];
			A[2+ii+lda*(jj+0)] = ptr_pA[2+bs*0];
			A[3+ii+lda*(jj+0)] = ptr_pA[3+bs*0];
			// unroll 0
			A[0+ii+lda*(jj+1)] = ptr_pA[0+bs*1];
			A[1+ii+lda*(jj+1)] = ptr_pA[1+bs*1];
			A[2+ii+lda*(jj+1)] = ptr_pA[2+bs*1];
			A[3+ii+lda*(jj+1)] = ptr_pA[3+bs*1];
			// unroll 0
			A[0+ii+lda*(jj+2)] = ptr_pA[0+bs*2];
			A[1+ii+lda*(jj+2)] = ptr_pA[1+bs*2];
			A[2+ii+lda*(jj+2)] = ptr_pA[2+bs*2];
			A[3+ii+lda*(jj+2)] = ptr_pA[3+bs*2];
			// unroll 0
			A[0+ii+lda*(jj+3)] = ptr_pA[0+bs*3];
			A[1+ii+lda*(jj+3)] = ptr_pA[1+bs*3];
			A[2+ii+lda*(jj+3)] = ptr_pA[2+bs*3];
			A[3+ii+lda*(jj+3)] = ptr_pA[3+bs*3];
			ptr_pA += sda*bs;
			}
		for(; ii<m; ii++)
			{
			// unroll 0
			A[ii+lda*(jj+0)] = ptr_pA[0+bs*0];
			// unroll 1
			A[ii+lda*(jj+1)] = ptr_pA[0+bs*1];
			// unroll 2
			A[ii+lda*(jj+2)] = ptr_pA[0+bs*2];
			// unroll 3
			A[ii+lda*(jj+3)] = ptr_pA[0+bs*3];
			ptr_pA++;
			}
		}
	for(; jj<n; jj++)
		{
		ptr_pA = pA + jj*bs;
		ii = 0;
		if(m0>0)
			{
			for(; ii<m0; ii++)
				{
				A[ii+lda*jj] = ptr_pA[0];
				ptr_pA++;
				}
			ptr_pA += (sda-1)*bs;
			}
		for(; ii<m-bs+1; ii+=bs)
			{
			A[0+ii+lda*jj] = ptr_pA[0];
			A[1+ii+lda*jj] = ptr_pA[1];
			A[2+ii+lda*jj] = ptr_pA[2];
			A[3+ii+lda*jj] = ptr_pA[3];
			ptr_pA += sda*bs;
			}
		for(; ii<m; ii++)
			{
			A[ii+lda*jj] = ptr_pA[0];
			ptr_pA++;
			}
		}
	return;
	}


// convert and transpose a matrix structure into a matrix
void blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda)
	{
	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + aj*bs + ai/bs*bs*sda + ai%bs;
	int i, ii, jj;
	int m0 = (bs-ai%bs)%bs;
	if(m0>m)
		m0 = m;
	float *ptr_pA;
	jj=0;
	for(; jj<n-3; jj+=4)
		{
		ptr_pA = pA + jj*bs;
		ii = 0;
		if(m0>0)
			{
			for(; ii<m0; ii++)
				{
				// unroll 0
				A[jj+0+lda*ii] = ptr_pA[0+bs*0];
				// unroll 1
				A[jj+1+lda*ii] = ptr_pA[0+bs*1];
				// unroll 2
				A[jj+2+lda*ii] = ptr_pA[0+bs*2];
				// unroll 3
				A[jj+3+lda*ii] = ptr_pA[0+bs*3];
				ptr_pA++;
				}
			ptr_pA += (sda-1)*bs;
			}
		for(; ii<m-bs+1; ii+=bs)
			{
			// unroll 0
			A[jj+0+lda*(ii+0)] = ptr_pA[0+bs*0];
			A[jj+0+lda*(ii+1)] = ptr_pA[1+bs*0];
			A[jj+0+lda*(ii+2)] = ptr_pA[2+bs*0];
			A[jj+0+lda*(ii+3)] = ptr_pA[3+bs*0];
			// unroll 1
			A[jj+1+lda*(ii+0)] = ptr_pA[0+bs*1];
			A[jj+1+lda*(ii+1)] = ptr_pA[1+bs*1];
			A[jj+1+lda*(ii+2)] = ptr_pA[2+bs*1];
			A[jj+1+lda*(ii+3)] = ptr_pA[3+bs*1];
			// unroll 2
			A[jj+2+lda*(ii+0)] = ptr_pA[0+bs*2];
			A[jj+2+lda*(ii+1)] = ptr_pA[1+bs*2];
			A[jj+2+lda*(ii+2)] = ptr_pA[2+bs*2];
			A[jj+2+lda*(ii+3)] = ptr_pA[3+bs*2];
			// unroll 3
			A[jj+3+lda*(ii+0)] = ptr_pA[0+bs*3];
			A[jj+3+lda*(ii+1)] = ptr_pA[1+bs*3];
			A[jj+3+lda*(ii+2)] = ptr_pA[2+bs*3];
			A[jj+3+lda*(ii+3)] = ptr_pA[3+bs*3];
			ptr_pA += sda*bs;
			}
		for(; ii<m; ii++)
			{
			// unroll 0
			A[jj+0+lda*ii] = ptr_pA[0+bs*0];
			// unroll 1
			A[jj+1+lda*ii] = ptr_pA[0+bs*1];
			// unroll 2
			A[jj+2+lda*ii] = ptr_pA[0+bs*2];
			// unroll 3
			A[jj+3+lda*ii] = ptr_pA[0+bs*3];
			ptr_pA++;
			}
		}
	for(; jj<n; jj++)
		{
		ptr_pA = pA + jj*bs;
		ii = 0;
		if(m0>0)
			{
			for(; ii<m0; ii++)
				{
				A[jj+lda*ii] = ptr_pA[0];
				ptr_pA++;
				}
			ptr_pA += (sda-1)*bs;
			}
		for(; ii<m-bs+1; ii+=bs)
			{
			i=0;
			for(; i<bs; i++)
				{
				A[jj+lda*(i+ii)] = ptr_pA[0];
				ptr_pA++;
				}
			ptr_pA += (sda-1)*bs;
			}
		for(; ii<m; ii++)
			{
			A[jj+lda*ii] = ptr_pA[0];
			ptr_pA++;
			}
		}
	return;
	}


// convert a vector structure into a vector
void blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *a)
	{
	float *pa = sa->pa + ai;
	int ii;
	for(ii=0; ii<m; ii++)
		a[ii] = pa[ii];
	return;
	}


// cast a matrix into a matrix structure
void s_cast_mat2strmat(float *A, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	sA->pA = A;
	return;
	}


// cast a matrix into the diagonal of a matrix structure
void s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	sA->dA = dA;
	return;
	}


// cast a vector into a vector structure
void s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa)
	{
	sa->pa = a;
	return;
	}


// insert element into strmat
void blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj)
	{

	if (ai == aj)
		{
		// invalidate stored inverse diagonal
		sA->use_dA = 0;
		}

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	pA[0] = a;
	return;
	}


// extract element from strmat
float blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj)
	{
	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	return pA[0];
	}


// insert element into strvec
void blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi)
	{
	const int bs = 4;
	float *x = sx->pa + xi;
	x[0] = a;
	return;
	}


// extract element from strvec
float blasfeo_svecex1(struct blasfeo_svec *sx, int xi)
	{
	const int bs = 4;
	float *x = sx->pa + xi;
	return x[0];
	}


// set all elements of a strmat to a value
void blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai%bs + ai/bs*bs*sda + aj*bs;
	int m0 = m<(bs-ai%bs)%bs ? m : (bs-ai%bs)%bs;
	int ii, jj;
	if(m0>0)
		{
		for(ii=0; ii<m0; ii++)
			{
			for(jj=0; jj<n; jj++)
				{
				pA[jj*bs] = alpha;
				}
			pA += 1;
			}
		pA += bs*(sda-1);
		m -= m0;
		}
	for(ii=0; ii<m-3; ii+=4)
		{
		for(jj=0; jj<n; jj++)
			{
			pA[0+jj*bs] = alpha;
			pA[1+jj*bs] = alpha;
			pA[2+jj*bs] = alpha;
			pA[3+jj*bs] = alpha;
			}
		pA += bs*sda;
		}
	for( ; ii<m; ii++)
		{
		for(jj=0; jj<n; jj++)
			{
			pA[jj*bs] = alpha;
			}
		pA += 1;
		}
	return;
	}


// set all elements of a strvec to a value
void blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi)
	{
	float *x = sx->pa + xi;
	int ii;
	for(ii=0; ii<m; ii++)
		x[ii] = alpha;
	return;
	}


// extract diagonal to vector
void blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi)
	{
	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	sdiaex_lib(kmax, alpha, ai%bs, pA, sda, x);
	return;
	}


// insert a vector into diagonal
void blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	int offsetA = ai%bs;

	int kna = (bs-offsetA%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pA[ll+bs*ll] = alpha*x[ll];
			}
		pA += kna + bs*(sda-1) + kna*bs;
		x  += kna;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pA[jj*sda+(jj+0)*bs+0] = alpha*x[jj+0];
		pA[jj*sda+(jj+1)*bs+1] = alpha*x[jj+1];
		pA[jj*sda+(jj+2)*bs+2] = alpha*x[jj+2];
		pA[jj*sda+(jj+3)*bs+3] = alpha*x[jj+3];
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pA[jj*sda+(jj+ll)*bs+ll] = alpha*x[jj+ll];
		}
	return;
	}


// add scalar to diagonal
void blasfeo_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int offsetA = ai%bs;

	int kna = (bs-offsetA%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pA[ll+bs*ll] += alpha;
			}
		pA += kna + bs*(sda-1) + kna*bs;
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pA[jj*sda+(jj+0)*bs+0] += alpha;
		pA[jj*sda+(jj+1)*bs+1] += alpha;
		pA[jj*sda+(jj+2)*bs+2] += alpha;
		pA[jj*sda+(jj+3)*bs+3] += alpha;
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pA[jj*sda+(jj+ll)*bs+ll] += alpha;
		}
	return;
	}


// swap two rows of two matrix structs
void blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;
	sC->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int sdc = sC->cn;
	float *pC = sC->pA + ci/bs*bs*sdc + ci%bs + cj*bs;
	srowsw_lib(kmax, pA, pC);
	return;
	}


// permute the rows of a matrix struct
void blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	int ii;
	for(ii=0; ii<kmax; ii++)
		{
		if(ipiv[ii]!=ii)
			blasfeo_srowsw(sA->n, sA, ii, 0, sA, ipiv[ii], 0);
		}
	return;
	}


// inverse permute the rows of a matrix struct
void blasfeo_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	int ii;
	for(ii=kmax-1; ii>=0; ii--)
		{
		if(ipiv[ii]!=ii)
			blasfeo_srowsw(sA->n, sA, ii, 0, sA, ipiv[ii], 0);
		}
	return;
	}


// extract a row int a vector
void blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi)
	{
	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	srowex_lib(kmax, alpha, pA, x);
	return;
	}


// insert a vector into a row
void blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	srowin_lib(kmax, alpha, x, pA);
	return;
	}


// add a vector to a row
void blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	srowad_lib(kmax, alpha, x, pA);
	return;
	}


// extract vector from column
void blasfeo_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	scolex_lib(kmax, ai%bs, pA, sda, x);
	return;
	}


// insert as vector as a column
void blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	scolin_lib(kmax, x, ai%bs, pA, sda);
	return;
	}


// add scaled vector to column
void blasfeo_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	float *x = sx->pa + xi;
	scolad_lib(kmax, alpha, x, ai%bs, pA, sda);
	return;
	}


// scale a column
void blasfeo_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	const int bs = 4;

	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;

	int kna = (bs-ai%bs)%bs;
	kna = kmax<kna ? kmax : kna;

	int jj, ll;

	if(kna>0)
		{
		for(ll=0; ll<kna; ll++)
			{
			pA[ll] *= alpha;
			}
		pA += kna + bs*(sda-1);
		kmax -= kna;
		}
	for(jj=0; jj<kmax-3; jj+=4)
		{
		pA[jj*sda+0] *= alpha;
		pA[jj*sda+1] *= alpha;
		pA[jj*sda+2] *= alpha;
		pA[jj*sda+3] *= alpha;
		}
	for(ll=0; ll<kmax-jj; ll++)
		{
		pA[jj*sda+ll] *= alpha;
		}

	return;
	}


// swap two cols of two matrix structs
void blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;
	sC->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int sdc = sC->cn;
	float *pC = sC->pA + ci/bs*bs*sdc + ci%bs + cj*bs;
	scolsw_lib(kmax, ai%bs, pA, sda, ci%bs, pC, sdc);
	return;
	}


// permute the cols of a matrix struct
void blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	int ii;
	for(ii=0; ii<kmax; ii++)
		{
		if(ipiv[ii]!=ii)
			blasfeo_scolsw(sA->m, sA, 0, ii, sA, 0, ipiv[ii]);
		}
	return;
	}


// inverse permute the cols of a matrix struct
void blasfeo_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA)
	{

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

	int ii;
	for(ii=kmax-1; ii>=0; ii--)
		{
		if(ipiv[ii]!=ii)
			blasfeo_scolsw(sA->m, sA, 0, ii, sA, 0, ipiv[ii]);
		}
	return;
	}


// --- ge


// scale a generic strmat
void blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj)
	{

	if(m<=0 | n<=0)
		return;

	// invalidate stored inverse diagonal
	sA->use_dA = 0;

#if defined(DIM_CHECK)
	// non-negative size
	if(m<0) printf("\n****** blasfeo_sgesc : m<0 : %d<0 *****\n", m);
	if(n<0) printf("\n****** blasfeo_sgesc : n<0 : %d<0 *****\n", n);
	// non-negative offset
	if(ai<0) printf("\n****** blasfeo_sgesc : ai<0 : %d<0 *****\n", ai);
	if(aj<0) printf("\n****** blasfeo_sgesc : aj<0 : %d<0 *****\n", aj);
	// inside matrix
	// A: m x n
	if(ai+m > sA->m) printf("\n***** blasfeo_sgesc : ai+m > row(A) : %d+%d > %d *****\n", ai, m, sA->m);
	if(aj+n > sA->n) printf("\n***** blasfeo_sgesc : aj+n > col(A) : %d+%d > %d *****\n", aj, n, sA->n);
#endif

	const int bs = 4;

	int mna, ii;

	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + aj*bs;
	int offA = ai%bs;

	// same alignment
	ii = 0;
	// clean up at the beginning
	mna = (4-offA)%bs;
	if(mna>0)
		{
		if(m<mna) // mna<=3  ==>  m = { 1, 2 }
			{
			if(m==1)
				{
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pA+offA);
				return;
				}
			else //if(m==2 && mna==3)
				{
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pA+offA);
				return;
				}
			}
		if(mna==1)
			{
			kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pA+offA);
			pA += 4*sda;
			ii += 1;
			}
		else if(mna==2)
			{
			kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pA+offA);
			pA += 4*sda;
			ii += 2;
			}
		else // if(mna==3)
			{
			kernel_sgecpsc_3_0_lib4(n, &alpha, pA+offA, pA+offA);
			pA += 4*sda;
			ii += 3;
			}
		}
	// main loop
	for(; ii<m-3; ii+=4)
		{
		kernel_sgecpsc_4_0_lib4(n, &alpha, pA, pA);
		pA += 4*sda;
		}
	// clean up at the end
	if(ii<m)
		{
		if(m-ii==1)
			kernel_sgecpsc_1_0_lib4(n, &alpha, pA, pA);
		else if(m-ii==2)
			kernel_sgecpsc_2_0_lib4(n, &alpha, pA, pA);
		else // if(m-ii==3)
			kernel_sgecpsc_3_0_lib4(n, &alpha, pA, pA);
		}

	return;

	}


// copy and scale a generic strmat into a generic strmat
void blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj)
	{

	if(m<=0 | n<=0)
		return;

	// invalidate stored inverse diagonal
	sB->use_dA = 0;

#if defined(DIM_CHECK)
	// non-negative size
	if(m<0) printf("\n****** blasfeo_sgecpsc : m<0 : %d<0 *****\n", m);
	if(n<0) printf("\n****** blasfeo_sgecpsc : n<0 : %d<0 *****\n", n);
	// non-negative offset
	if(ai<0) printf("\n****** blasfeo_sgecpsc : ai<0 : %d<0 *****\n", ai);
	if(aj<0) printf("\n****** blasfeo_sgecpsc : aj<0 : %d<0 *****\n", aj);
	if(bi<0) printf("\n****** blasfeo_sgecpsc : bi<0 : %d<0 *****\n", bi);
	if(bj<0) printf("\n****** blasfeo_sgecpsc : bj<0 : %d<0 *****\n", bj);
	// inside matrix
	// A: m x n
	if(ai+m > sA->m) printf("\n***** blasfeo_sgecpsc : ai+m > row(A) : %d+%d > %d *****\n", ai, m, sA->m);
	if(aj+n > sA->n) printf("\n***** blasfeo_sgecpsc : aj+n > col(A) : %d+%d > %d *****\n", aj, n, sA->n);
	// B: m x n
	if(bi+m > sB->m) printf("\n***** blasfeo_sgecpsc : bi+m > row(B) : %d+%d > %d *****\n", bi, m, sB->m);
	if(bj+n > sB->n) printf("\n***** blasfeo_sgecpsc : bj+n > col(B) : %d+%d > %d *****\n", bj, n, sB->n);
#endif

	const int bs = 4;

	int mna, ii;

	int sda = sA->cn;
	int sdb = sB->cn;
	float *pA = sA->pA + ai/bs*bs*sda + aj*bs;
	float *pB = sB->pA + bi/bs*bs*sdb + bj*bs;
	int offA = ai%bs;
	int offB = bi%bs;

	// same alignment
	if(offA==offB)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecpsc_3_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgecpsc_4_0_lib4(n, &alpha, pA, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA, pB);
			else if(m-ii==2)
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA, pB);
			else // if(m-ii==3)
				kernel_sgecpsc_3_0_lib4(n, &alpha, pA, pB);
			}
		}
	// skip one element of pA
	else if(offA==(offB+1)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				//pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecpsc_2_3_lib4(n, &alpha, pA, sda, pB+2);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecpsc_3_2_lib4(n, &alpha, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for( ; ii<m-3; ii+=4)
			{
			kernel_sgecpsc_4_1_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+1, pB);
			else if(m-ii==2)
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA+1, pB);
			else // if(m-ii==3)
				kernel_sgecpsc_3_0_lib4(n, &alpha, pA+1, pB);
			}
		}
	// skip 2 elements of pA
	else if(offA==(offB+2)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgecpsc_2_3_lib4(n, &alpha, pA, sda, pB+1);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+1, pB+3);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA, pB+2);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecpsc_3_3_lib4(n, &alpha, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgecpsc_4_2_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+2, pB);
			else if(m-ii==2)
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA+2, pB);
			else // if(m-ii==3)
				kernel_sgecpsc_3_2_lib4(n, &alpha, pA, sda, pB);
			}
		}
	// skip 3 elements of pA
	else // if(offA==(offB+3)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;

		if(mna>0)
			{

			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}

			if(mna==1)
				{
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecpsc_2_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecpsc_3_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}

			}

		// main loop

		for(; ii<m-3; ii+=4)
			{
			kernel_sgecpsc_4_3_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}

		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecpsc_1_0_lib4(n, &alpha, pA+3, pB);
			else if(m-ii==2)
				kernel_sgecpsc_2_3_lib4(n, &alpha, pA, sda, pB);
			else // if(m-ii==3)
				kernel_sgecpsc_3_3_lib4(n, &alpha, pA, sda, pB);
			}
		}

	return;

	}


// copy a generic strmat into a generic strmat
void blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj)
	{

	if(m<=0 | n<=0)
		return;

	// invalidate stored inverse diagonal
	sB->use_dA = 0;

#if defined(DIM_CHECK)
	// non-negative size
	if(m<0) printf("\n****** blasfeo_sgecp : m<0 : %d<0 *****\n", m);
	if(n<0) printf("\n****** blasfeo_sgecp : n<0 : %d<0 *****\n", n);
	// non-negative offset
	if(ai<0) printf("\n****** blasfeo_sgecp : ai<0 : %d<0 *****\n", ai);
	if(aj<0) printf("\n****** blasfeo_sgecp : aj<0 : %d<0 *****\n", aj);
	if(bi<0) printf("\n****** blasfeo_sgecp : bi<0 : %d<0 *****\n", bi);
	if(bj<0) printf("\n****** blasfeo_sgecp : bj<0 : %d<0 *****\n", bj);
	// inside matrix
	// A: m x n
	if(ai+m > sA->m) printf("\n***** blasfeo_sgecp : ai+m > row(A) : %d+%d > %d *****\n", ai, m, sA->m);
	if(aj+n > sA->n) printf("\n***** blasfeo_sgecp : aj+n > col(A) : %d+%d > %d *****\n", aj, n, sA->n);
	// B: m x n
	if(bi+m > sB->m) printf("\n***** blasfeo_sgecp : bi+m > row(B) : %d+%d > %d *****\n", bi, m, sB->m);
	if(bj+n > sB->n) printf("\n***** blasfeo_sgecp : bj+n > col(B) : %d+%d > %d *****\n", bj, n, sB->n);
#endif

	const int bs = 4;

	int mna, ii;

	int sda = sA->cn;
	int sdb = sB->cn;
	float *pA = sA->pA + ai/bs*bs*sda + aj*bs;
	float *pB = sB->pA + bi/bs*bs*sdb + bj*bs;
	int offA = ai%bs;
	int offB = bi%bs;

	// same alignment
	if(offA==offB)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgecp_2_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecp_2_0_lib4(n, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecp_3_0_lib4(n, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgecp_4_0_lib4(n, pA, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecp_1_0_lib4(n, pA, pB);
			else if(m-ii==2)
				kernel_sgecp_2_0_lib4(n, pA, pB);
			else // if(m-ii==3)
				kernel_sgecp_3_0_lib4(n, pA, pB);
			}
		}
	// skip one element of pA
	else if(offA==(offB+1)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgecp_2_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
				//pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecp_2_3_lib4(n, pA, sda, pB+2);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecp_3_2_lib4(n, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for( ; ii<m-3; ii+=4)
			{
			kernel_sgecp_4_1_lib4(n, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecp_1_0_lib4(n, pA+1, pB);
			else if(m-ii==2)
				kernel_sgecp_2_0_lib4(n, pA+1, pB);
			else // if(m-ii==3)
				kernel_sgecp_3_0_lib4(n, pA+1, pB);
			}
		}
	// skip 2 elements of pA
	else if(offA==(offB+2)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgecp_2_3_lib4(n, pA, sda, pB+1);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecp_1_0_lib4(n, pA+1, pB+3);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecp_2_0_lib4(n, pA, pB+2);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecp_3_3_lib4(n, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgecp_4_2_lib4(n, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecp_1_0_lib4(n, pA+2, pB);
			else if(m-ii==2)
				kernel_sgecp_2_0_lib4(n, pA+2, pB);
			else // if(m-ii==3)
				kernel_sgecp_3_2_lib4(n, pA, sda, pB);
			}
		}
	// skip 3 elements of pA
	else // if(offA==(offB+3)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgecp_2_0_lib4(n, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgecp_1_0_lib4(n, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgecp_2_0_lib4(n, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgecp_3_0_lib4(n, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgecp_4_3_lib4(n, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgecp_1_0_lib4(n, pA+3, pB);
			else if(m-ii==2)
				kernel_sgecp_2_3_lib4(n, pA, sda, pB);
			else // if(m-ii==3)
				kernel_sgecp_3_3_lib4(n, pA, sda, pB);
			}
		}

	return;

	}


// scale a strvec
void blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai)
	{
	float *pa = sa->pa + ai;
	int ii;
	ii = 0;
	for(; ii<m-3; ii+=4)
		{
		pa[ii+0] *= alpha;
		pa[ii+1] *= alpha;
		pa[ii+2] *= alpha;
		pa[ii+3] *= alpha;
		}
	for(; ii<m; ii++)
		{
		pa[ii+0] *= alpha;
		}
	return;
	}


// copy a strvec into a strvec
void blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci)
	{
	float *pa = sa->pa + ai;
	float *pc = sc->pa + ci;
	int ii;
	ii = 0;
	for(; ii<m-3; ii+=4)
		{
		pc[ii+0] = pa[ii+0];
		pc[ii+1] = pa[ii+1];
		pc[ii+2] = pa[ii+2];
		pc[ii+3] = pa[ii+3];
		}
	for(; ii<m; ii++)
		{
		pc[ii+0] = pa[ii+0];
		}
	return;
	}


// copy and scale a strvec into a strvec
void blasfeo_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci)
	{
	float *pa = sa->pa + ai;
	float *pc = sc->pa + ci;
	int ii;
	ii = 0;
	for(; ii<m-3; ii+=4)
		{
		pc[ii+0] = alpha*pa[ii+0];
		pc[ii+1] = alpha*pa[ii+1];
		pc[ii+2] = alpha*pa[ii+2];
		pc[ii+3] = alpha*pa[ii+3];
		}
	for(; ii<m; ii++)
		{
		pc[ii+0] = alpha*pa[ii+0];
		}
	return;
	}


// copy a lower triangular strmat into a lower triangular strmat
void blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj)
	{

	if(m<=0)
		return;

	// invalidate stored inverse diagonal
	sB->use_dA = 0;

	const int bs = 4;

	int sda = sA->cn;
	int sdb = sB->cn;
	float *pA = sA->pA + ai/bs*bs*sda + aj*bs;
	float *pB = sB->pA + bi/bs*bs*sdb + bj*bs;
	int offA = ai%bs;
	int offB = bi%bs;

	int ii, mna;

	// same alignment
	if(offA==offB)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_strcp_l_2_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_strcp_l_2_0_lib4(ii, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_strcp_l_3_0_lib4(ii, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_strcp_l_4_0_lib4(ii, pA, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_strcp_l_1_0_lib4(ii, pA, pB);
			else if(m-ii==2)
				kernel_strcp_l_2_0_lib4(ii, pA, pB);
			else // if(m-ii==3)
				kernel_strcp_l_3_0_lib4(ii, pA, pB);
			}
		}
	// skip one element of pA
	else if(offA==(offB+1)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_strcp_l_2_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
				//pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_strcp_l_2_3_lib4(ii, pA, sda, pB+2);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_strcp_l_3_2_lib4(ii, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for( ; ii<m-3; ii+=4)
			{
			kernel_strcp_l_4_1_lib4(ii, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_strcp_l_1_0_lib4(ii, pA+1, pB);
			else if(m-ii==2)
				kernel_strcp_l_2_0_lib4(ii, pA+1, pB);
			else // if(m-ii==3)
				kernel_strcp_l_3_0_lib4(ii, pA+1, pB);
			}
		}
	// skip 2 elements of pA
	else if(offA==(offB+2)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_strcp_l_2_3_lib4(ii, pA, sda, pB+1);
					return;
					}
				}
			if(mna==1)
				{
				kernel_strcp_l_1_0_lib4(ii, pA+1, pB+3);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_strcp_l_2_0_lib4(ii, pA, pB+2);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_strcp_l_3_3_lib4(ii, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_strcp_l_4_2_lib4(ii, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_strcp_l_1_0_lib4(ii, pA+2, pB);
			else if(m-ii==2)
				kernel_strcp_l_2_0_lib4(ii, pA+2, pB);
			else // if(m-ii==3)
				kernel_strcp_l_3_2_lib4(ii, pA, sda, pB);
			}
		}
	// skip 3 elements of pA
	else // if(offA==(offB+3)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_strcp_l_2_0_lib4(ii, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_strcp_l_1_0_lib4(ii, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_strcp_l_2_0_lib4(ii, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_strcp_l_3_0_lib4(ii, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_strcp_l_4_3_lib4(ii, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_strcp_l_1_0_lib4(ii, pA+3, pB);
			else if(m-ii==2)
				kernel_strcp_l_2_3_lib4(ii, pA, sda, pB);
			else // if(m-ii==3)
				kernel_strcp_l_3_3_lib4(ii, pA, sda, pB);
			}
		}

	return;

	}


// scale and add a generic strmat into a generic strmat
void blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj)
	{

	if(m<=0 || n<=0)
		return;

	// invalidate stored inverse diagonal
	sB->use_dA = 0;

	const int bs = 4;

	int sda = sA->cn;
	int sdb = sB->cn;
	float *pA = sA->pA + ai/bs*bs*sda + aj*bs;
	float *pB = sB->pA + bi/bs*bs*sdb + bj*bs;
	int offA = ai%bs;
	int offB = bi%bs;

	int ii, mna;

	// same alignment
	if(offA==offB)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgead_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgead_2_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgead_3_0_lib4(n, &alpha, pA+offA, pB+offB);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgead_4_0_lib4(n, &alpha, pA, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgead_1_0_lib4(n, &alpha, pA, pB);
			else if(m-ii==2)
				kernel_sgead_2_0_lib4(n, &alpha, pA, pB);
			else // if(m-ii==3)
				kernel_sgead_3_0_lib4(n, &alpha, pA, pB);
			}
		}
	// skip one element of pA
	else if(offA==(offB+1)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna) // mna<=3  ==>  m = { 1, 2 }
				{
				if(m==1)
					{
					kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else //if(m==2 && mna==3)
					{
					kernel_sgead_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				//pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgead_2_3_lib4(n, &alpha, pA, sda, pB+2);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgead_3_2_lib4(n, &alpha, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for( ; ii<m-3; ii+=4)
			{
			kernel_sgead_4_1_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgead_1_0_lib4(n, &alpha, pA+1, pB);
			else if(m-ii==2)
				kernel_sgead_2_0_lib4(n, &alpha, pA+1, pB);
			else // if(m-ii==3)
				kernel_sgead_3_0_lib4(n, &alpha, pA+1, pB);
			}
		}
	// skip 2 elements of pA
	else if(offA==(offB+2)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgead_2_3_lib4(n, &alpha, pA, sda, pB+1);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgead_1_0_lib4(n, &alpha, pA+1, pB+3);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgead_2_0_lib4(n, &alpha, pA, pB+2);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgead_3_3_lib4(n, &alpha, pA, sda, pB+1);
				pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgead_4_2_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgead_1_0_lib4(n, &alpha, pA+2, pB);
			else if(m-ii==2)
				kernel_sgead_2_0_lib4(n, &alpha, pA+2, pB);
			else // if(m-ii==3)
				kernel_sgead_3_2_lib4(n, &alpha, pA, sda, pB);
			}
		}
	// skip 3 elements of pA
	else // if(offA==(offB+3)%bs)
		{
		ii = 0;
		// clean up at the beginning
		mna = (4-offB)%bs;
		if(mna>0)
			{
			if(m<mna)
				{
				if(m==1)
					{
					kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				else // if(m==2 && mna==3)
					{
					kernel_sgead_2_0_lib4(n, &alpha, pA+offA, pB+offB);
					return;
					}
				}
			if(mna==1)
				{
				kernel_sgead_1_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 1;
				}
			else if(mna==2)
				{
				kernel_sgead_2_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 2;
				}
			else // if(mna==3)
				{
				kernel_sgead_3_0_lib4(n, &alpha, pA+offA, pB+offB);
				// pA += 4*sda;
				pB += 4*sdb;
				ii += 3;
				}
			}
		// main loop
		for(; ii<m-3; ii+=4)
			{
			kernel_sgead_4_3_lib4(n, &alpha, pA, sda, pB);
			pA += 4*sda;
			pB += 4*sdb;
			}
		// clean up at the end
		if(ii<m)
			{
			if(m-ii==1)
				kernel_sgead_1_0_lib4(n, &alpha, pA+3, pB);
			else if(m-ii==2)
				kernel_sgead_2_3_lib4(n, &alpha, pA, sda, pB);
			else // if(m-ii==3)
				kernel_sgead_3_3_lib4(n, &alpha, pA, sda, pB);
			}
		}

	return;

	}


// copy and transpose a generic strmat into a generic strmat
void blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj)
	{
	if(m<=0 || n<=0)
		return;

	// invalidate stored inverse diagonal
	sC->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int sdc = sC->cn;
	float *pC = sC->pA + ci/bs*bs*sdc + ci%bs + cj*bs;
	sgetr_lib(m, n, 1.0, ai%bs, pA, sda, ci%bs, pC, sdc); // TODO remove alpha !!!
	return;
	}


// copy and transpose a lower triangular strmat into an upper triangular strmat
void blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj)
	{
	if(m<=0)
		return;

	// invalidate stored inverse diagonal
	sC->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int sdc = sC->cn;
	float *pC = sC->pA + ci/bs*bs*sdc + ci%bs + cj*bs;
	strtr_l_lib(m, 1.0, ai%bs, pA, sda, ci%bs, pC, sdc); // TODO remove alpha !!!
	return;
	}


// copy and transpose an upper triangular strmat into a lower triangular strmat
void blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj)
	{
	if(m<=0)
		return;

	// invalidate stored inverse diagonal
	sC->use_dA = 0;

	const int bs = 4;
	int sda = sA->cn;
	float *pA = sA->pA + ai/bs*bs*sda + ai%bs + aj*bs;
	int sdc = sC->cn;
	float *pC = sC->pA + ci/bs*bs*sdc + ci%bs + cj*bs;
	strtr_u_lib(m, 1.0, ai%bs, pA, sda, ci%bs, pC, sdc); // TODO remove alpha !!!
	return;
	}


// insert a strvec to diagonal of strmat, sparse formulation
void blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj)
	{
	if(kmax<=0)
		return;

	// invalidate stored inverse diagonal
	sD->use_dA = 0;

	const int bs = 4;
	float *x = sx->pa + xi;
	int sdd = sD->cn;
	float *pD = sD->pA;
	int ii, jj;
	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[(ii+di)/bs*bs*sdd+(ii+di)%bs+(ii+dj)*bs] = alpha * x[jj];
		}
	return;
	}


// extract the diagonal of a strmat to a strvec, sparse formulation
void blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi)
	{
	const int bs = 4;
	float *x = sx->pa + xi;
	int sdd = sD->cn;
	float *pD = sD->pA;
	int ii, jj;
	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		x[jj] = alpha * pD[(ii+di)/bs*bs*sdd+(ii+di)%bs+(ii+dj)*bs];
		}
	return;
	}


// add scaled strvec to diagonal of strmat, sparse formulation
void blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj)
	{
	if(kmax<=0)
		return;

	// invalidate stored inverse diagonal
	sD->use_dA = 0;

	const int bs = 4;
	float *x = sx->pa + xi;
	int sdd = sD->cn;
	float *pD = sD->pA;
	int ii, jj;
	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[(ii+di)/bs*bs*sdd+(ii+di)%bs+(ii+dj)*bs] += alpha * x[jj];
		}
	return;
	}


// add scaled strvec to another strvec and insert to diagonal of strmat, sparse formulation
void blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj)
	{
	if(kmax<=0)
		return;

	// invalidate stored inverse diagonal
	sD->use_dA = 0;

	const int bs = 4;
	float *x = sx->pa + xi;
	float *y = sy->pa + yi;
	int sdd = sD->cn;
	float *pD = sD->pA;
	int ii, jj;
	for(jj=0; jj<kmax; jj++)
		{
		ii = idx[jj];
		pD[(ii+di)/bs*bs*sdd+(ii+di)%bs+(ii+dj)*bs] = y[jj] + alpha * x[jj];
		}
	return;
	}


// add scaled strvec to row of strmat, sparse formulation
void blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj)
	{
	if(kmax<=0)
		return;

	// invalidate stored inverse diagonal
	sD->use_dA = 0;

	const int bs = 4;
	float *x = sx->pa + xi;
	int sdd = sD->cn;
	float *pD = sD->pA + di/bs*bs*sdd + di%bs + dj*bs;
	srowad_libsp(kmax, idx, alpha, x, pD);
	return;
	}


// adds strvec to strvec, sparse formulation
void blasfeo_svecad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sy, int yi)
	{
	float *x = sx->pa + xi;
	float *y = sy->pa + yi;
	svecad_libsp(kmax, idx, alpha, x, y);
	return;
	}


void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi)
	{
	float *x = sx->pa + xi;
	float *z = sz->pa + zi;
	int ii;
	for(ii=0; ii<m; ii++)
		z[idx[ii]] = alpha * x[ii];
	return;
	}


void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi)
	{
	float *x = sx->pa + xi;
	float *z = sz->pa + zi;
	int ii;
	for(ii=0; ii<m; ii++)
		z[ii] = alpha * x[idx[ii]];
	return;
	}


void blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm)
	{
	int ii;
	float *x = sx->pa + xi;
	float norm = 0.0;
	float tmp;
	for(ii=0; ii<m; ii++)
		{
#ifdef USE_C99_MATH
		norm = fmaxf(norm, fabsf(x[ii]));
#else
		tmp = fabs(x[ii]);
		norm = tmp>norm ? tmp : norm;
#endif
		}
	*ptr_norm = norm;
	return;
	}


// permute elements of a vector struct
void blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi)
	{
	int ii;
	float tmp;
	float *x = sx->pa + xi;
	for(ii=0; ii<kmax; ii++)
		{
		if(ipiv[ii]!=ii)
			{
			tmp = x[ipiv[ii]];
			x[ipiv[ii]] = x[ii];
			x[ii] = tmp;
			}
		}
	return;
	}


// inverse permute elements of a vector struct
void blasfeo_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi)
	{
	int ii;
	float tmp;
	float *x = sx->pa + xi;
	for(ii=kmax-1; ii>=0; ii--)
		{
		if(ipiv[ii]!=ii)
			{
			tmp = x[ipiv[ii]];
			x[ipiv[ii]] = x[ii];
			x[ii] = tmp;
			}
		}
	return;
	}


#else

#error : wrong LA choice

#endif