int p3dZeroPadding2D_8(
	unsigned char* in_im,
	unsigned char* out_im,
	const int dimx, // ncols
	const int dimy, // nrows
	const int size,
	int (*wr_log)(const char*, ...),
	int (*wr_progress)(const int, ...)
	) 
{
	int a_dimx, a_dimy;
	int i, j;

	// Compute dimensions of padded REV:
	a_dimx = dimx + size * 2;
	a_dimy = dimy + size * 2;

	// Set to zero all values:
	memset(out_im, 0, a_dimx * a_dimy * sizeof (unsigned char));

	// Copy original (internal) values:
	for (j = 0; j < dimy; j++) {
		for (i = 0; i < dimx; i++) {
			out_im[ I2(i + size, j + size, a_dimx) ] = in_im[ I2(i, j, dimx) ];
		}
	}

	return P3D_SUCCESS;
}

void Postprocessing::initialise()
{
	// Read in the input maps
	fn_I1 = fn_in + "_half1_class001_unfil.mrc";
	fn_I2 = fn_in + "_half2_class001_unfil.mrc";

	if (verb > 0)
	{
		std::cout <<"== Reading input half-reconstructions: " <<std::endl;
		std::cout.width(35); std::cout << std::left <<"  + half1-map: "; std::cout << fn_I1 << std::endl;
		std::cout.width(35); std::cout << std::left <<"  + half2-map: "; std::cout << fn_I2 << std::endl;
	}

	I1.read(fn_I1);
	I2.read(fn_I2);
	I1().setXmippOrigin();
	I2().setXmippOrigin();

	if (!I1().sameShape(I2()))
	{
		std::cerr << " Size of half1 map: "; I1().printShape(std::cerr); std::cerr << std::endl;
		std::cerr << " Size of half2 map: "; I2().printShape(std::cerr); std::cerr << std::endl;
		REPORT_ERROR("Postprocessing::initialise ERROR: The two half reconstructions are not of the same size!");
	}

	if (do_auto_mask && fn_mask != "")
		REPORT_ERROR("Postprocessing::initialise ERROR: provide either --auto_mask OR --mask, but not both!");

	if (do_auto_bfac && ABS(adhoc_bfac) > 0.)
		REPORT_ERROR("Postprocessing::initialise ERROR: provide either --auto_bfac OR --adhoc_bfac, but not both!");
}

static void reset_locks(void)
{
	local_irq_disable();
	I1(A); I1(B); I1(C); I1(D);
	I1(X1); I1(X2); I1(Y1); I1(Y2); I1(Z1); I1(Z2);
	lockdep_reset();
	I2(A); I2(B); I2(C); I2(D);
	init_shared_classes();
	local_irq_enable();
}

void nemo_main ()
{
  setparams();	/* get cmdline stuff and compute x,y,u,v,etot,lz */

  optr = NULL;				/* make an orbit */
  allocate_orbit (&optr, 3, 1);
  Masso(optr)  = 1.0;                     /* and set Mass */
  Key(optr) = 0;
  Torb(optr,0) = tnow;
  Xorb(optr,0) = x;			/* .. positions */
  Yorb(optr,0) = y;
  Zorb(optr,0) = z;
  Uorb(optr,0) = u;			/* .. velocities */
  Vorb(optr,0) = v;
  Worb(optr,0) = w;
  I1(optr) = etot;			/*  energy (zero if not used) */
  I2(optr) = lz;                          /* angular momentum */
  
  dprintf(0,"pos: %f %f %f  \nvel: %f %f %f  \netot: %f\nlz=%f\n",
	  x,y,z,u,v,w,etot,lz);

  outstr = stropen (outfile,"w");		/* write to file */
  put_history(outstr);
  PotName(optr) = p.name;
  PotPars(optr) = p.pars;
  PotFile(optr) = p.file;
  write_orbit(outstr,optr);
  strclose(outstr);
}

Mat Assignment2::computeH(const vector<Point2f> &query, const vector<Point2f> &train)
{
	Mat I1(3,4,CV_64FC1,0.0);
	Mat I2(3,4,CV_64FC1,0.0);
	Mat H(3,3,CV_64FC1,0.0);
	
	for(unsigned int i=0; i<(query.size()); i++)
	{
		// Assign co-ordinates to the matrix
		I1.at<double>(0,i)=query[i].x;
		I1.at<double>(1,i)=query[i].y;
		I1.at<double>(2,i)=1;
		
		I2.at<double>(0,i)=train[i].x;
		I2.at<double>(1,i)=train[i].y;
		I2.at<double>(2,i)=1;
	}
	
	// solve linear equations
	solve(I1.t(), I2.t(), H, DECOMP_SVD);

	H = H.t();

	return H;
}

void TerrainPageSurfaceLayer::populate(const TerrainPageGeometry& geometry)
{
	const SegmentVector validSegments = geometry.getValidSegments();
	for (SegmentVector::const_iterator I = validSegments.begin(); I != validSegments.end(); ++I) {
#if 0
		//the current Mercator code works such that whenever an Area is added to Terrain, _all_ surfaces for the affected segments are invalidated, thus requiering a total repopulation of the segment
		//If however that code was changed to only invalidate the affected surface the code below would be very much handy
		Mercator::Surface* surface(getSurfaceForSegment(I->segment));
		if (surface) {
			surface->populate();
		}
#else

		Mercator::Segment* segment(I->segment);
		if (!segment->isValid()) {
			segment->populate();
		}

		Mercator::Segment::Surfacestore::iterator I2(segment->getSurfaces().find(mSurfaceIndex));
		if (I2 == segment->getSurfaces().end()) {
			//the segment doesn't contain this surface yet, lets add it
			if (mShader.checkIntersect(*segment)) {
				S_LOG_VERBOSE("Adding new surface with id " << mSurfaceIndex << " to segment at x: " << segment->getXRef() << " y: " << segment->getYRef());
				Mercator::Segment::Surfacestore & sss = segment->getSurfaces();
				sss[mSurfaceIndex] = mShader.newSurface(*segment);
			}
		}
		//NOTE: we have to repopulate all surfaces mainly to get the foliage to work.
		segment->populateSurfaces();
#endif
	}
}

tensor MDYieldSurface::xi_t1( const EPState *EPS) const {
  tensor dFoverds( 2, def_dim_2, 0.0);
  tensor I2("I", 2, def_dim_2);

  stresstensor S = EPS->getStress().deviator();

  double p = EPS->getStress().p_hydrostatic();
 
  stresstensor n = EPS->getTensorVar( 2 );
  //stresstensor n;
  	    
  ////-------------------------------------------------
  //double m = EPS->getScalarVar( 1 );
  //stresstensor alpha = EPS->getTensorVar( 1 ); // getting alpha_ij from EPState  
  //stresstensor r = S * (1.0 / p);
  //stresstensor r_bar = r - alpha;
  //stresstensor norm2 = r_bar("ij") * r_bar("ij");
  //double norm = sqrt( norm2.trace() );
  //
  //if ( norm >= d_macheps() ){ 
  //  n = r_bar *(1.0 / norm );
  //}
  //else {
  //  opserr->fatal("MDYieldSurface::dFods  |n_ij| = 0, divide by zero! Program exits.");
  //  exit(-1);
  //}
  //n = r_bar *(1.0 / sqrt23rd / m );
  ////-------------------------------------------------
    
  return  n  * (-1.0)* p;
}

ImageScale::ImageScale(const Image<float>& I,double r){
  IntegralImages inter(I);
  radius_size=r;
  step=sqrt(2.0);
  int size= std::max(I.Width(),I.Height());

  int number= int(log(size/r)/log(2.0))+1;
  angles.resize(number);
  magnitudes.resize(number);
  ratios.resize(number);


  GradAndNorm(I,angles[0],magnitudes[0]);
  ratios[0]=1;

//#ifdef _OPENMP
//#pragma omp parallel for
//#endif
  for (int i=1; i<number; i++){
    Image<float> I2;
    double ratio=1*pow(step,i);  
   
    I2.Resize(int(I.Width()/ratio), int(I.Height()/ratio));
    angles[i].Resize(int(I.Width()/ratio), int(I.Height()/ratio));
    magnitudes[i].Resize(int(I.Width()/ratio), int(I.Height()/ratio));

    for (int i=0;i<I2.Width(); i++){
      for (int j=0;j<I2.Height();j++){
        I2(j,i)=inter(double(i+0.5)*ratio,double(j+0.5)*ratio,ratio);
      }
    }
    GradAndNorm(I2,angles[i],magnitudes[i]);
    ratios[i]=ratio;
  }
}

tensor DPYieldSurface01::xi_t1( const EPState *EPS) const {
    tensor dFoverds( 2, def_dim_2, 0.0);
    tensor I2("I", 2, def_dim_2);

    stresstensor S = EPS->getStress().deviator();

    double p = EPS->getStress().p_hydrostatic();
    p = p - Pc;

    stresstensor alpha = EPS->getTensorVar( 1 ); // getting alpha_ij from EPState

    stresstensor r = S * (1.0 / p); //for p = sig_kk/3
    stresstensor r_bar = r - alpha;
    stresstensor norm2 = r_bar("ij") * r_bar("ij");
    double norm = sqrt( norm2.trace() );

    stresstensor n;
    if ( norm >= d_macheps() ) {
        n = r_bar *(1.0 / norm );
    }
    else {
        opserr << "DPYieldSurface01::dFods  |n_ij| = 0, divide by zero! Program exits.\n";
        exit(-1);
    }

    return (-1.0) * n * p;
}

static void reset_locks(void)
{
	local_irq_disable();
	lockdep_free_key_range(&ww_lockdep.acquire_key, 1);
	lockdep_free_key_range(&ww_lockdep.mutex_key, 1);

	I1(A); I1(B); I1(C); I1(D);
	I1(X1); I1(X2); I1(Y1); I1(Y2); I1(Z1); I1(Z2);
	I_WW(t); I_WW(t2); I_WW(o.base); I_WW(o2.base); I_WW(o3.base);
	lockdep_reset();
	I2(A); I2(B); I2(C); I2(D);
	init_shared_classes();

	ww_mutex_init(&o, &ww_lockdep); ww_mutex_init(&o2, &ww_lockdep); ww_mutex_init(&o3, &ww_lockdep);
	memset(&t, 0, sizeof(t)); memset(&t2, 0, sizeof(t2));
	memset(&ww_lockdep.acquire_key, 0, sizeof(ww_lockdep.acquire_key));
	memset(&ww_lockdep.mutex_key, 0, sizeof(ww_lockdep.mutex_key));
	local_irq_enable();
}

void Solver::DiagramI2(){

    double time0,time1;
    time0 = omp_get_wtime();

    #pragma omp parallel
    {
        int id = omp_get_thread_num();
        int threads = omp_get_num_threads();

        for (int n=id; n<Nphhp; n+=threads) blocksphhp[n]->SetUpMatrices_I2(t0);
    }
    time1 = omp_get_wtime(); //cout << "Inside I2. SetUpMatrices needs: " << time1-time0 << " seconds" << endl;

    time0 = omp_get_wtime();

    for (int n=0; n<Nphhp; n++){

        mat I2 =  blocksphhp[n]->I2 * blocksphhp[n]->T / blocksphhp[n]->epsilon;

        for (int x1=0; x1<blocksphhp[n]->Nph; x1++){
            for (int x2=0; x2<blocksphhp[n]->Nph; x2++){

                int i=blocksphhp[n]->Xph(x1,1); int j=blocksphhp[n]->Xhp(x2,0);
                int a=blocksphhp[n]->Xph(x1,0); int b=blocksphhp[n]->Xhp(x2,1);

                t( Index(a,b,i,j) ) += I2(x1,x2);
                t( Index(a,b,j,i) ) -= I2(x1,x2);
                t( Index(b,a,i,j) ) -= I2(x1,x2);
                t( Index(b,a,j,i) ) += I2(x1,x2);

                //StoreT( Index(a,b,i,j), I2(x1,x2));
                //StoreT( Index(a,b,j,i), -I2(x1,x2));
                //StoreT( Index(b,a,i,j), -I2(x1,x2));
                //StoreT( Index(b,a,j,i), I2(x1,x2));
            }
        }
    }

    time1 = omp_get_wtime(); //cout << "Inside I1. Calculate matrices needs: " << time1-time0 << " seconds" << endl;
}

int decode_navgpstime(tUbxRawData *raw)
{
   unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
   tExternalGNSSGPSTimePtr gpstime = Msg_Get_ExternalGnssGpsTime();

   /* do not reset this struct due to week and leapsecond */
   gpstime->iTow = U4(p);
   gpstime->fTow = I4(p+4);
   gpstime->gpsWeek = (short)(I2(p + 8));
   gpstime->leapSec = (short)(I1(p + 10));

   return 0; 
}

int _getSums_16(
	unsigned short* p_in_im,
	unsigned char* mask_im,
	unsigned char* p_mask_im,
	int p_dim,
	int i,
	int j,
	int winsize,
	double* sum,
	double* sqrsum
	) 
{
	int k;

	// Init sums:
	*sum = 0.0;
	*sqrsum = 0.0;

	// Cycle for each element in line to compute mean and
	// variance of the line. This computation is meaningless
	// if line is not completely included into ROI.
	for (k = i; k < (i + winsize); k++) {

		// Check if element is outside ROI:
		if (mask_im != NULL) {
			if (p_mask_im[ I2(k, j, p_dim) ] == 0)
				// If element is outside ROI break computation to improve
					// performance:
						return 0; // false
		}

		// Compute sums for further use in mean and variance
		// computation:
		*sum += p_in_im[ I2(k, j, p_dim) ];
		*sqrsum += p_in_im[ I2(k, j, p_dim) ] * p_in_im[ I2(k, j, p_dim) ];
	}

	return 1; // true
}

void bench_env_klip(Pt2di sz)
{
     Im2D_U_INT1 I1(sz.x,sz.y);
     Im2D_U_INT1 I2(sz.x,sz.y);
     INT vmax = 30;




     ELISE_COPY
     (
         I1.all_pts(),
         Min
         (
             vmax,
                1
             +  frandr()*5
             +  unif_noise_4(3) * 30
         ),
         I1.out() 
     );
     ELISE_COPY
     (
         I1.border(1),
         0,
         I1.out()
     );
     ELISE_COPY
     (
         I1.all_pts(),
         EnvKLipshcitz_32(I1.in(0),vmax),
         I2.out() 
     );


     U_INT1 ** i1 = I1.data();
     U_INT1 ** i2 = I2.data();

     for (INT x=0; x<sz.x ; x++)
         for (INT y=0; y<sz.y ; y++)
            if (i1[y][x])
            {
                 INT v  = std::min3 
                      (
                          std::min3(i2[y+1][x-1]+3,i2[y+1][x]+2,i2[y+1][x+1]+3),
                          std::min3(i2[y][x-1]+2,(INT)i1[y][x],i2[y][x+1]+2),
                          std::min3(i2[y-1][x-1]+3,i2[y-1][x]+2,i2[y-1][x+1]+3)
                      );
                 BENCH_ASSERT(v==i2[y][x]);
           }
}

bool Postprocessing::getMask()
{

	// A. Check whether a user-provided mask is to be used
	if (do_auto_mask)
	{
		getAutoMask();

	}
	else if (fn_mask != "")
	{
		if (verb > 0)
		{
			std::cout << "== Using a user-provided mask ... " <<std::endl;
			std::cout.width(35); std::cout << std::left   << "  + input mask: "; std::cout  << fn_mask <<std::endl;
		}

		// Read the mask in memory
		Im.read(fn_mask);
		Im().setXmippOrigin();

		// Check values are between 0 and 1
		DOUBLE avg, stddev, minval, maxval;
		Im().computeStats(avg, stddev, minval, maxval);
		if (minval < -1e-6 || maxval - 1. > 1.e-6)
		{
			std::cerr << " minval= " << minval << " maxval= " << maxval << std::endl;
			REPORT_ERROR("Postprocessing::mask ERROR: mask values not in range [0,1]!");
		}

		// Also check the mask is the same size as the input maps
		if (!Im().sameShape(I2()))
		{
			std::cerr << " Size of input mask: "; Im().printShape(std::cerr); std::cerr<< std::endl;
			std::cerr << " Size of input maps: "; I1().printShape(std::cerr); std::cerr<< std::endl;
			REPORT_ERROR("Postprocessing::mask ERROR: mask and input maps do not have the same size!");
		}

	}
	else
	{
		if (verb > 0)
		{
			std::cout << "== Not performing any masking ... " << std::endl;
		}
		return false;
	}

	return true;
}

/* decode NVS x4btime --------------------------------------------------------*/
static int decode_x4btime(raw_t *raw)
{
    unsigned char *p=raw->buff+2;
    
    trace(4,"decode_x4btime: len=%d\n", raw->len);
    
    raw->nav.utc_gps[1] = R8(p   );
    raw->nav.utc_gps[0] = R8(p+ 8);
    raw->nav.utc_gps[2] = I4(p+16);
    raw->nav.utc_gps[3] = I2(p+20);
    raw->nav.leaps = I1(p+22);
    
    return 9;
}

//======================================================================
int EightNode_Brick_u_p::update()
{
    int ret = 0;
    int where;
    int i,j;
    
    tensor I2("I", 2, def_dim_2);
    
    double r  = 0.0;
    double s  = 0.0;
    double t  = 0.0;

    int tdisp_dim[] = {Num_Nodes,Num_Dim};
    tensor total_disp(2,tdisp_dim,0.0);

    int dh_dim[] = {Num_Nodes, Num_Dim};
    tensor dh(2, dh_dim, 0.0);

    straintensor strn;

    tensor t_disp = getNodesDisp();

    for (i=1; i<=Num_Nodes; i++) {
      for (j=1; j<=Num_Dim; j++) {
        total_disp.val(i,j) = t_disp.cval(i,j);
      }
    }

    int GP_c_r, GP_c_s, GP_c_t;

    for( GP_c_r = 0 ; GP_c_r < Num_IntegrationPts; GP_c_r++ ) {
      r = pts[GP_c_r];
      for( GP_c_s = 0 ; GP_c_s < Num_IntegrationPts; GP_c_s++ ) {
        s = pts[GP_c_s];
        for( GP_c_t = 0 ; GP_c_t < Num_IntegrationPts; GP_c_t++ ) {
          t = pts[GP_c_t];
          where = (GP_c_r*Num_IntegrationPts+GP_c_s)*Num_IntegrationPts+GP_c_t;
          dh = dh_Global(r,s,t);
          strn = total_disp("Ia")*dh("Ib");
          strn.null_indices();
          strn.symmetrize11();
          if ( (theMaterial[where]->setTrialStrain(strn) ) )
            opserr << "EightNode_Brick_u_p::update (tag: " << this->getTag() << "), not converged\n";
        }
      }
    }

  return ret;
}

/* decode ephemeris ----------------------------------------------------------*/
static int decode_gpsephem(int sat, raw_t *raw)
{
    eph_t eph={0};
    unsigned char *puiTmp = (raw->buff)+2;
    unsigned short week;
    double toc;
    
    trace(4,"decode_ephem: sat=%2d\n",sat);
    
    eph.crs    = R4(&puiTmp[  2]);
    eph.deln   = R4(&puiTmp[  6]) * 1e+3;
    eph.M0     = R8(&puiTmp[ 10]);
    eph.cuc    = R4(&puiTmp[ 18]);
    eph.e      = R8(&puiTmp[ 22]);
    eph.cus    = R4(&puiTmp[ 30]);
    eph.A      = pow(R8(&puiTmp[ 34]), 2);
    eph.toes   = R8(&puiTmp[ 42]) * 1e-3;
    eph.cic    = R4(&puiTmp[ 50]);
    eph.OMG0   = R8(&puiTmp[ 54]);
    eph.cis    = R4(&puiTmp[ 62]);
    eph.i0     = R8(&puiTmp[ 66]);
    eph.crc    = R4(&puiTmp[ 74]);
    eph.omg    = R8(&puiTmp[ 78]);
    eph.OMGd   = R8(&puiTmp[ 86]) * 1e+3;
    eph.idot   = R8(&puiTmp[ 94]) * 1e+3;
    eph.tgd[0] = R4(&puiTmp[102]) * 1e-3;
    toc        = R8(&puiTmp[106]) * 1e-3;
    eph.f2     = R4(&puiTmp[114]) * 1e+3;
    eph.f1     = R4(&puiTmp[118]);
    eph.f0     = R4(&puiTmp[122]) * 1e-3;
    eph.sva    = uraindex(I2(&puiTmp[126]));
    eph.iode   = I2(&puiTmp[128]);
    eph.iodc   = I2(&puiTmp[130]);
    eph.code   = I2(&puiTmp[132]);
    eph.flag   = I2(&puiTmp[134]);
    week       = I2(&puiTmp[136]);
    eph.fit    = 0;
    
    if (week>=4096) {
        trace(2,"nvs gps ephemeris week error: sat=%2d week=%d\n",sat,week);
        return -1;
    }
    eph.week=adjgpsweek(week);
    eph.toe=gpst2time(eph.week,eph.toes);
    eph.toc=gpst2time(eph.week,toc);
    eph.ttr=raw->time;
    
    if (!strstr(raw->opt,"-EPHALL")) {
        if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
    }
    eph.sat=sat;
    raw->nav.eph[sat-1]=eph;
    raw->ephsat=sat;
    return 2;
}

int main() {

  Nef_polyhedron N1(Plane_3( 1, 0, 0,-1),Nef_polyhedron::INCLUDED);
  Nef_polyhedron N2(Plane_3(-1, 0, 0,-1),Nef_polyhedron::INCLUDED);
  Nef_polyhedron N3(Plane_3( 0, 1, 0,-1),Nef_polyhedron::INCLUDED);
  Nef_polyhedron N4(Plane_3( 0,-1, 0,-1),Nef_polyhedron::INCLUDED);
  Nef_polyhedron N5(Plane_3( 0, 0, 1,-1),Nef_polyhedron::INCLUDED);
  Nef_polyhedron N6(Plane_3( 0, 0,-1,-1),Nef_polyhedron::INCLUDED);

  Nef_polyhedron I1(!N1+!N2);
  Nef_polyhedron I2(N3-!N4);
  Nef_polyhedron I3(N5^N6);
  Nef_polyhedron Cube1(I2 *!I1);
  Cube1 *= !I3;
  Nef_polyhedron Cube2 = N1 * N2 * N3 * N4 * N5 * N6;
  CGAL_assertion (Cube1 == Cube2);
  return 0;
}

tensor CAMYieldSurface::dFods(const EPState *EPS) const {
  
  tensor dFoverds( 2, def_dim_2, 0.0);
  tensor I2("I", 2, def_dim_2);

  double p = EPS->getStress().p_hydrostatic();
  double q = EPS->getStress().q_deviatoric();
  double po = EPS->getScalarVar( 1 );
	 tensor DpoDs = EPS->getStress().dpoverds();
	 tensor DqoDs = EPS->getStress().dqoverds();

  double dFoverdp = -1.0*M*M*( po - 2.0*p );
  double dFoverdq = 2.0*q;

  dFoverds = DpoDs  * dFoverdp +
             DqoDs  * dFoverdq;

  return dFoverds;

}