CMatrix::MatrixError_t   CMatrix::init (int iNumRows, int iNumCols)
{
    int i;

    assert (iNumRows >= 0);
    assert (iNumCols >= 0);

    if (iNumRows == m_aiMatrixDimensions[kRow] && iNumCols == m_aiMatrixDimensions[kCol])
    {
        setZero();
        return kMatrixNoError;
    }
    else
        reset();

    m_ppfMatrix = new float* [iNumRows];

    if (!m_ppfMatrix)
        return kMatrixMemAllocError;

    for (i = 0; i < iNumRows; i++)
    {
        m_ppfMatrix[i]  = new float [iNumCols];
        if (!m_ppfMatrix[i])
            return kMatrixMemAllocError;
    }

    m_aiMatrixDimensions[kRow]  = iNumRows;
    m_aiMatrixDimensions[kCol]  = iNumCols;

    setZero();

    return kMatrixNoError;
}

bool GainClient::disablePositionForceControlGains(double transition_duration, bool wait_for_success)
{
  sl_controller_msgs::CartesianGains position_gains;
  setZero(position_gains);
  sl_controller_msgs::CartesianGains force_gains;
  setZero(force_gains);
  return setArmPositionForceControlGains(position_gains, force_gains, transition_duration, wait_for_success);
}

void initializeA(float A[][M][maxDegree+1], float Acopy[][M][maxDegree+1]) {
	int i, n, m;
	float temp;
	for(n = 0; n < N; ++n) {
		for(m = 0; m < M; ++m) {
			setZero(A[n][m]);
			setZero(Acopy[n][m]);
		}
	}
	int choice;
	printf("If you would like to manually enter a matrix of polynomials, press 0.\nIf you would like have one generated randomly, press 1: ");
	scanf("%d", &choice);
	while (choice != 0 && choice != 1) {
		printf("\nPlease enter 0 (manual) or 1 (random): ");
		scanf("%d", &choice);
	}

	if (choice == 1) {
		float negative;
		for(n = 0; n < N; ++n) {
			for(m = 0; m < M; ++m) {
				for(i = 0; i <= maxEntry; ++i) {
					// generate a number that is either -1 or +1
					negative = (float) (rand()%2);
					negative = negative - 0.5;
					negative *= 2;

					// generate a random coefficient
					temp = (float) (rand()%10);
					temp *= negative;

					A[n][m][i] = temp;
					Acopy[n][m][i] = temp;
				}
			}
		}
	}

	else {
		printf("Okay, please initialize the matrix \"A\".\n");
		for(n = 0; n < N; ++n) {
			for(m = 0; m < M; ++m) {
				for(i = 0; i <= maxEntry; ++i) {
					printf("Enter the coefficient of x^%d in A[%d][%d]: ", i, n, m);
					scanf("%f", &temp);
					A[n][m][i] = temp;
					Acopy[n][m][i] = temp;
				}
			}
		}
	}
	
}

void Decimal::setReal(double value)
{
    setZero();

    int    dec;
    int    sign;
    char digitText[DOUBLE_SIG_DIGITS+2];
    if (!safe_ecvt(sizeof(digitText), digitText, value, DOUBLE_SIG_DIGITS, &dec, &sign))
        return;

    int len = DOUBLE_SIG_DIGITS;
    int hi = zeroDigit - 1 + dec;
    int lo  = hi - (len -1);

    const char * finger = digitText;
    //Number too big - should it create a maximum value? or truncate as it currently does.
    if (hi >= maxDigits)  // Most of this work is dealing with out of range cases
    {
        if (lo >= maxDigits)
            return;
        
        finger += (hi - (maxDigits-1));
        hi = maxDigits-1;
    }
    
    if (lo < 0)
    {
        if (hi < 0)
            return;
        
        lo = 0;
    }
    
    msb = hi;
    lsb  = lo;
    
    for ( int i = hi; i >= lo; i-- )
    {
        byte next = *finger++ - '0';
        if (next < 10)
            digits[i] = next;
        else
        {
            //infinity????
            setZero();
            return;
        }
    }

    if (sign)
        negative = true;
}

aol::Vector<_DataType>::Vector ( aol::Vector<_DataType> const&Vec, CopyFlag copyFlag )
    : Obj ( Vec ), _size ( Vec._size ), _sizeReserved ( Vec._sizeReserved ),
    overflowHandling ( Vec.overflowHandling ), overflowMin ( Vec.overflowMin ), overflowMax ( Vec.overflowMax ) {
  switch ( copyFlag ) {
    case DEEP_COPY:
      _pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
      _deleteFlag = true;
      memcpy ( _pData, Vec._pData, _size*sizeof ( DataType ) );
      break;

    case FLAT_COPY:
      _pData = Vec._pData;
      _deleteFlag = false;
      //     _size = Vec._size; // this happens in the initialization list.
      //     _sizeReserved = Vec._sizeReserved;
      break;

    case STRUCT_COPY:
      _pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
      _deleteFlag = true;
      setZero(); // same as in standard constructor: by convention, vectors are initialized with zero.
      break;

    case STRUCT_COPY_UNINIT:
      _pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
      _deleteFlag = true;
      // do not set to zero
      break;

    default:
      throw aol::Exception ( "Invalid CopyFlag specified", __FILE__, __LINE__ );
      break;
  };
}

bool runRalfFunction(std::string name, scalarFnType fun, CUmodule* hModule, CUdeviceptr d_data,
		     DataStruct *h_data,DataStruct* h_data_reference, unsigned int memSize)
{
  const unsigned inputNr = 10;
  const float scalarInputs[4][inputNr] = {{ 0.f, 3.f, 2.f, 8.f, 10.2f, -1.f, 0.f, 1000.23f, 0.02f, -0.02f },
					   { 1.f, 2.f, 4.f, 6.f, -14.13f, -13.f, 0.f, 0.02f, 420.001f, -420.001f },
					   { 2.f, 1.f, 6.f, 4.f, 999.f, -5.f, 0.f, 420.001f, 0.01f, 0.01f },
					   { 3.f, 0.f, 8.f, 2.f, 0.f, -420.001f, 0.f, 0.01f, 1000.23f, 0.01f }};


  std::cout << "====================== " << name << "===============================\n";
  for (unsigned i=0; i<inputNr; ++i) {
    for (unsigned j=0; j<inputNr; ++j) 
    for (unsigned k=0; k<4; ++k) 
    {
      setZero(h_data,h_data_reference);
      h_data->fa[0] = h_data_reference->fa[0] = scalarInputs[k][i];
      h_data->fa[1] = h_data_reference->fa[1] = scalarInputs[k][j];

      //run device function
      loadAndRunTestFunction(hModule, name, d_data, h_data, memSize);   
      fun(h_data_reference);

      if(!compareData(h_data, h_data_reference))                      //compare Data
      {
	std::cout << "\n Error in Ralf: fa0=" << scalarInputs[k][i]  
                  << ", fa1=" << scalarInputs[k][j] << " (" << name << ")\n";
	return false;
      }
    }
  }
  std::cout << " => Test passed!!!\n";
  return true;
}

// set the identity matrix of dimension n
void Matrix::setIdentityMatrix( int n )		
{
    assert( n > 0 );
    setDimensions( n, n );	
    setZero();
    for ( int i = 0; i < n; i++ ) setElement( i, i, 1.0 );
}

aol::Vector<_DataType>::Vector ( const qc::GridStructure &Grid ) : _size ( Grid.getNumberOfNodes() ), _sizeReserved ( _size ), _deleteFlag ( true ), overflowHandling ( aol::CLIP ), overflowMin ( 0 ), overflowMax ( aol::OverflowTrait<DataType>::max ) {
  _pData = static_cast<DataType*> ( aol::MemoryManager::allocateAtLeast ( _sizeReserved, sizeof ( DataType ) ) );
  if ( !_pData )
    throw OutOfMemoryException ( "Vector<DataType,Realtype>::Vector: Could not allocate memory for Vector.", __FILE__, __LINE__ );

  setZero ();
}

//Add with carry
void adc(CPU *c, OP_CODE_INFO *o){
    int8_t carry = getFlag(c,C);
    int8_t accum = getRegByte(c,ACCUM);
    int8_t operand = o->operand;
    int16_t sum = (0x00FF&carry) + (0x00FF&accum) + (0x00FF&operand);
    int8_t sumByte = sum & 0x00FF;
    setZero(c,sumByte);
    if(getFlag(c,D)){ //in decimal mode
        //if lower 4 bits of operands plus
        //the carry in are larger than 9,
        //then we need to apply conversions
        //to remain in binary coded decimal format.
        if((accum & 0xF) + (operand & 0xF)
            + carry > 9){
            sum += 6;
        }
        setSign(c,sum&0xFF);
        setOverflow(c,accum,operand,sum&0xFF);
        //if the higher bits aren't in
        //BCD format we need to add 96 to convert.
        //Black magic from http://nesdev.com/6502.txt
        sum += sum > 0x99 ? 96 : 0;
        setCarryBCD(c, sum);
    } else {
        setSign(c,sumByte);
        setOverflow(c,accum,operand,sumByte);
        setCarry(c,sum);
    }
    setRegByte(c,ACCUM,sum&0xFF);
}

void TMatrix3::setIdentity()
{
  setZero();
  v_[0][0].coeff(0) = 1;
  v_[1][1].coeff(0) = 1;
  v_[2][2].coeff(0) = 1;

}

// set the diagonal matrix
void Matrix::setDiagonalMatrix( const vector< double >& diag )
{
    int n = diag.size();
    
    setDimensions( n, n );
    setZero();
    for ( int i = 0; i < n; i++ ) setElement( i, i, diag[ i ] );
}

void and(CPU *c, OP_CODE_INFO *o){
    int8_t accum = getRegByte(c,ACCUM);
    int8_t operand = o->operand;
    int8_t res = accum & operand;
    setSign(c,res);
    setZero(c,res);
    setRegByte(c,ACCUM,res);
}

Point Manifold::getZero() const
{
  mnf_assert(isValid() || seeMessageAbove());
  lock();
  Eigen::VectorXd id(representationDim_);
  setZero(id);
  return Point(*this, id);
}

void initializeA(float A[][M][maxDegree+1], float Acopy[][M][maxDegree+1]) {
	int i, n, m;
	for(n = 0; n < N; ++n) {
		for(m = 0; m < M; ++m) {
			setZero(A[n][m]);
			setZero(Acopy[n][m]);
		}
	}

	// A[0][0][0] = 1;
	// A[0][0][1] = 0;
	// A[0][0][2] = 1;
	// A[0][1][0] = 0;
	// A[0][1][1] = -4;
	// A[0][1][2] = 3;
	// A[1][0][0] = 0;
	// A[1][0][1] = 8;
	// A[1][0][2] = 0;
	// A[1][1][0] = 0;
	// A[1][1][1] = 0;
	// A[1][1][2] = -1;

	float temp;
	float negative;
	// printf("Initialize the matrix \"A\".\n");
	for(n = 0; n < N; ++n) {
		for(m = 0; m < M; ++m) {
			for(i = 0; i <= maxEntry; ++i) {
				// printf("Enter the coefficient of x^%d in A[%d][%d]: ", i, n, m);
				// scanf("%f", &temp);

				//after this, negative will be either +1 or -1
				negative = (float) (rand()%2);
				negative = negative - 0.5;
				negative *= 2;

				temp = (float) (rand()%10);
				temp *= negative;

				A[n][m][i] = temp;
				Acopy[n][m][i] = temp;
			}
		}
	}
	
}

void ossimMatrix4x4::setIdentity()
{
   setZero();
   theData[0][0] = 1.0;
   theData[1][1] = 1.0;
   theData[2][2] = 1.0;
   theData[3][3] = 1.0;
}

Decimal & Decimal::multiply(const Decimal & other)
{
    int low1, high1, low2, high2, lowt, hight;

    clip(low1, high1);
    other.clip(low2, high2);
    lowt = low1+low2-zeroDigit;
    if (lowt < 0) lowt = 0;
    hight = high1 + high2 - zeroDigit;
    if (hight >= maxDigits) hight = maxDigits-1;
    else if (hight < 0)
    {
        if (hight < -1)
        {
            setZero();
            return *this;
        }
        hight = 0;
    }


    unsigned temp[maxDigits*2];
    _clear(temp);
//  memset(temp+low1+low2, 0, (high1+high2-low1-low2+2)*sizeof(unsigned));  // only need to clear part of the target we're adding to.

    //More: could copy across 1st time round - might be worth it.
    const byte * digits1 = digits;
    const byte * digits2 = other.digits;
    for (int i = low1; i <= high1; i++)
    {
        byte next = digits1[i];
        if (next)
        {
            for (int j=low2; j <= high2; j++)
                temp[i+j] += next * digits2[j];
        }
    }

    //Now copy the results, taking cary of the carries 
    unsigned carry = 0;
    int j;
    for (j = low1+low2 - zeroDigit; j < lowt; j++)
        carry = (temp[j+zeroDigit]+carry)/10;
    for (j = lowt; j <= hight; j++)
    {
        div_t next = div(temp[j+zeroDigit]+carry, 10);
        digits[j] = next.rem;
        carry = next.quot;
    }
    if ((hight < maxDigits-1) && (carry != 0))
        digits[++hight] = carry % 10;

    lsb = lowt;
    msb = hight;
    negative ^= other.negative;
    return *this;
}

int main(int argc, char **argv)
{
	Show( Array);

	setZero( Array );

	Show( Array);

	return 0;
}

Mat4& Mat4::setIdentity()
{
    setZero();
    m_arrData[0][0] = 1;
    m_arrData[1][1] = 1;
    m_arrData[2][2] = 1;
    m_arrData[3][3] = 1;

    return *this;
}

//initializes P to identity
void initializeP(float P[][N][maxDegree+1]) {
	int i, j;
	for(i = 0; i < N; ++i) {
		for(j = 0; j < N; ++j) {
			setZero(P[i][j]);
			if (i == j) {
				P[i][j][0] = 1;
			}
		}
	}
}

// initializes Q to identity
void initializeQ(float Q[][M][maxDegree+1]) {
	int i, j;
	for(i = 0; i < M; ++i) {
		for(j = 0; j < M; ++j) {
			setZero(Q[i][j]);
			if (i == j) {
				Q[i][j][0] = 1;
			}
		}
	}
}