Esempio n. 1
0
void
VisualizeCurve (ON_NurbsCurve &curve, double r, double g, double b, bool show_cps)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, cloud, 8);

  for (std::size_t i = 0; i < cloud->size () - 1; i++)
  {
    pcl::PointXYZRGB &p1 = cloud->at (i);
    pcl::PointXYZRGB &p2 = cloud->at (i + 1);
    std::ostringstream os;
    os << "line_" << r << "_" << g << "_" << b << "_" << i;
    viewer.addLine<pcl::PointXYZRGB> (p1, p2, r, g, b, os.str ());
  }

  if (show_cps)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cps (new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < curve.CVCount (); i++)
    {
      ON_3dPoint cp;
      curve.GetCV (i, cp);

      pcl::PointXYZ p;
      p.x = float (cp.x);
      p.y = float (cp.y);
      p.z = float (cp.z);
      cps->push_back (p);
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler (cps, 255 * r, 255 * g, 255 * b);
    viewer.addPointCloud<pcl::PointXYZ> (cps, handler, "cloud_cps");
  }
}
Esempio n. 2
0
/*--------------------------------------------------------------------------*/
IsotimeEpoch _string2epoch(const char *epoch_s)
{ long base;
  IsotimeEpoch epoch;
  char epobuf[EPOLEN], *pr;
  char secbuf[SLEN], *ps;
  char frabuf[SLEN], *pf;

  long sec=0;
  double fract=0.0;

  if (ISOTIME_debug>0) fprintf(stderr,"_string2epoch >>%s<<\n",epoch_s);

  epoch.status = -1;
  epoch.sec    = 0;
  epoch.fract  = 0.0;
  epoch.offset = 0;

  // trim isotime_s, convert to uppercase and copy to trimmed
  ps = trim( epobuf, EPOLEN, epoch_s );
  if (ISOTIME_debug>1) fprintf(stderr," trim returns >>%s<<\n",ps);

  // copy seconds
  pf = cps( secbuf, SLEN, ps );
  if (ISOTIME_debug>1) fprintf(stderr," section >>%s<<\n",secbuf);

  // copy fraction
  pr = cpf( frabuf, SLEN, pf );
  if (ISOTIME_debug>1) fprintf(stderr," fraction >>%s<<\n",frabuf);

  if ( strlen(pr) ) { 
    // error, if rest is not empty
    fprintf( stderr, "ERROR: Cannot read epoch \"%s\"\n",epobuf );
    fprintf( stderr, "       Format: [+|-]sssssssssss[.uuuuuuu]\n" );
    fprintf( stderr, "               e.g. \"1149254287\", \"+1149254287.1\"\n");
    goto _string2epocherr;
  }

  sscanf(secbuf,"%ld",&sec);
  sscanf(frabuf,"%lf",&fract);

  epoch.sec = sec;
  if (sec>=0) epoch.fract = fract;
  epoch.fract = -fract;

  // normalize fract and sec
  base = floor(epoch.fract);
  epoch.fract -= (double) base;
  epoch.sec   += base;

  epoch.status = 0;

  if (ISOTIME_debug>0) fprintf(stderr,"_string2epoch END\n");

  return ( epoch );

_string2epocherr:
  if (ISOTIME_debug>0) fprintf(stderr,"_string2epoch END\n");
  return ( epoch );

} // _string2epoch
Esempio n. 3
0
CUIApp::CUIApp()
{
	// Place all significant initialization in InitInstance
	pause=false;
	pauseSecs=0;
	
	CPersistentString cps("RunBefore");
	FirstEverTimeRun=!cps.toBool();
	cps=true;

}
Esempio n. 4
0
bool ConstraintChecker::HasSupportPolygon(const Robot& robot,const Stance& stance,const Vector3& gravity,int numFCEdges) 
{
  vector<ContactPoint> cps(NumContactPoints(stance));
  int k=0;
  for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
    const Hold& h=i->second;
    for(size_t j=0;j<h.contacts.size();j++,k++)
      cps[k]=h.contacts[j];
  }
  Vector3 com = robot.GetCOM();
  vector<Vector3> f;
  return TestCOMEquilibrium(cps,gravity,numFCEdges,com,f);
}
Esempio n. 5
0
bool ConstraintChecker::HasSupportPolygon_Robust(const Robot& robot,const Stance& stance,const Vector3& gravity,Real robustnessFactor,int numFCEdges) 
{
  vector<ContactPoint> cps(NumContactPoints(stance));
  int k=0;
  for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
    const Hold& h=i->second;
    for(size_t j=0;j<h.contacts.size();j++,k++)
      cps[k]=h.contacts[j];
  }
  Vector3 com = robot.GetCOM();
  EquilibriumTester eq;
  eq.Setup(cps,gravity,numFCEdges,com);
  eq.SetRobustnessFactor(robustnessFactor);
  return eq.TestCurrent();
}
Esempio n. 6
0
//********************************************************************************
void CGuiHeaderCtrl::DrawArrow(CDC* pDC,CRect rc,BOOL bUp)
{
	CPen cp(PS_SOLID,1, m_clrFace);
	CPen cps(PS_SOLID,1, m_clrShadow);
	CPen cpw(PS_SOLID,1, m_clrLight);
	CPen *pOld;
	
	rc.left=rc.right-12;
	rc.right=rc.left+8;
	rc.bottom=rc.top+12;
	rc.top+=2;	
	int m_mitad=rc.left+4;
	
	if (bUp == TRUE)
	{
			//linea izquierda
			pOld=pDC->SelectObject(&cps);
			pDC->MoveTo(rc.left,rc.bottom);
			pDC->LineTo(m_mitad,rc.top);
			//linea derecha
			pDC->SelectObject(&cpw);
			pDC->MoveTo(rc.right,rc.bottom);
			pDC->LineTo(m_mitad,rc.top);
			//linea de abajo
			pDC->MoveTo(rc.left,rc.bottom);
			pDC->LineTo(rc.right,rc.bottom);
	}
	else
	{
			rc.bottom=rc.top+12;
			rc.top+=4;	
			//linea izquierda
			pOld=pDC->SelectObject(&cps);
			pDC->MoveTo(rc.left,rc.top);
			pDC->LineTo(m_mitad,rc.bottom);
			//linea superior
			pDC->MoveTo(rc.left,rc.top);
			pDC->LineTo(rc.right,rc.top);
			//linea derecha
				pDC->SelectObject(&cpw);
			pDC->MoveTo(rc.right,rc.top);
			pDC->LineTo(m_mitad,rc.bottom);

	}

	pDC->SelectObject(pOld);
}
Esempio n. 7
0
//************************************************************************
void CGuiHeaderCtrl::BiselaBoton(CRect rcWin,CDC* pDC)
{
	CPen cp(PS_SOLID,1, m_clrFace);
	CPen cps(PS_SOLID,1, m_clrShadow);
	CPen cpw(PS_SOLID,1, m_clrLight);

	//***************************************************
	pDC->Draw3dRect(rcWin,m_clrLight,m_clrShadow);
	rcWin.DeflateRect(1,1);
	pDC->Draw3dRect(rcWin,m_clrFace,m_clrFace);
	//***************************************************

	CPen* cpold=pDC->SelectObject(&cp);	
	int iNumItems=GetItemCount();
	int iContx=0;
	HDITEM m_hditems;
	
	for (int i =0; i < iNumItems; i++)
	{
		CRect recItem;
		GetItem(i,&m_hditems);
		GetItemRect(i, recItem);
		iContx+=recItem.Width();
		//quitamos todas las lineas
		recItem.DeflateRect(1,1);
		pDC->SelectObject(&cp);
		pDC->MoveTo(iContx-2,rcWin.top+1);
		pDC->LineTo(iContx-2,rcWin.bottom-1);
		pDC->MoveTo(iContx-1,rcWin.top+1);
		pDC->LineTo(iContx-1,rcWin.bottom-1);
		pDC->MoveTo(iContx,rcWin.top+1);
		pDC->LineTo(iContx,rcWin.bottom-1);
		pDC->MoveTo(iContx+1,rcWin.top+1);
		pDC->LineTo(iContx+1,rcWin.bottom-1);
		//ponemos dos para dar el efecto
		pDC->SelectObject(&cps);
		pDC->MoveTo(iContx-1,rcWin.top+2);
		pDC->LineTo(iContx-1,rcWin.bottom-2);
		pDC->SelectObject(&cpw);
		pDC->MoveTo(iContx,rcWin.top+2);
		pDC->LineTo(iContx,rcWin.bottom-2);
	
	}
	pDC->SelectObject(cpold);

}
//-----------------------------------------------------------------------------
// CaptivePortalService::nsIObserver
//-----------------------------------------------------------------------------
NS_IMETHODIMP
CaptivePortalService::Observe(nsISupports *aSubject,
                              const char * aTopic,
                              const char16_t * aData)
{
  if (XRE_GetProcessType() != GeckoProcessType_Default) {
    // Doesn't do anything if called in the content process.
    return NS_OK;
  }

  LOG(("CaptivePortalService::Observe() topic=%s\n", aTopic));
  if (!strcmp(aTopic, kOpenCaptivePortalLoginEvent)) {
    // A redirect or altered content has been detected.
    // The user needs to log in. We are in a captive portal.
    mState = LOCKED_PORTAL;
    mLastChecked = TimeStamp::Now();
    mEverBeenCaptive = true;
  } else if (!strcmp(aTopic, kCaptivePortalLoginSuccessEvent)) {
    // The user has successfully logged in. We have connectivity.
    mState = UNLOCKED_PORTAL;
    mLastChecked = TimeStamp::Now();
    mSlackCount = 0;
    mDelay = mMinInterval;

    RearmTimer();
  } else if (!strcmp(aTopic, kAbortCaptivePortalLoginEvent)) {
    // The login has been aborted
    mState = UNKNOWN;
    mLastChecked = TimeStamp::Now();
    mSlackCount = 0;
  }

  // Send notification so that the captive portal state is mirrored in the
  // content process.
  nsCOMPtr<nsIObserverService> observerService = services::GetObserverService();
  if (observerService) {
    nsCOMPtr<nsICaptivePortalService> cps(this);
    observerService->NotifyObservers(cps, NS_IPC_CAPTIVE_PORTAL_SET_STATE, nullptr);
  }

  return NS_OK;
}
/**
  * Edit selected profile preferences
  *
  *
  */
void CAMProfileSettingsList::on_bEdit_clicked(const QModelIndex &parent) {
    Q_UNUSED(parent);
    //ui->tableView->selectedItems();

     // Get all selections
    QModelIndexList indexes = ui->tableView->selectionModel()->selection().indexes();

    QList<int> list;
    for (int i = 0; i < indexes.count(); ++i)
    {
        list.append(indexes.at(i).row());
    }
    list=QSet<int>::fromList(list).toList();
    foreach(int row, list)
    {
        CAMProfileSettings cps(this);
        cps.setData(camProfileDataModel->getList().at(row));
        if (cps.exec()==QDialog::Accepted) {
            savePreferences();
        }
    }
int main(int argc, char** argv)
{
  ros::init(argc, argv, "collision_proximity_server_test");

  ros::AsyncSpinner spinner(1); 
  spinner.start();
 
  ros::NodeHandle nh;
  std::string robot_description_name = nh.resolveName("robot_description", true);

  CollisionProximitySpacePlannerInterface cps(robot_description_name);

  ros::Rate r(10.0);
  while(nh.ok()) {
    cps.broadcastCollisionMarkers();
    r.sleep();
  }
  ros::waitForShutdown();
  return 0;


}
Esempio n. 11
0
contractor_ibex_hc4::contractor_ibex_hc4(vector<Enode *> const & vars,
                                         vector<shared_ptr<nonlinear_constraint>> const & ctrs)
    : contractor_cell(contractor_kind::IBEX_HC4), m_ctrs(ctrs) {
    // Trivial Case
    if (m_ctrs.size() == 0) {
        return;
    }
    unsigned index = 0;
    ibex::Array<ibex::NumConstraint> cps(ctrs.size());
    for (shared_ptr<nonlinear_constraint> numctr : ctrs) {
        cps.set_ref(index++, *(numctr->get_numctr()));
    }
    m_ctc.reset(new ibex::CtcHC4(cps));
    unordered_map<Enode *, unsigned> enode_to_id;
    for (unsigned i = 0; i < vars.size(); ++i) {
        enode_to_id.emplace(vars[i], i);
    }
    for (shared_ptr<nonlinear_constraint> ctr : ctrs) {
        unordered_set<Enode *> const & vars_in_ctr = ctr->get_enode()->get_vars();
        m_vars_in_ctrs.insert(vars_in_ctr.begin(), vars_in_ctr.end());
    }
    DREAL_LOG_INFO << "contractor_ibex_hc4: DONE" << endl;
}
Esempio n. 12
0
 ///Randomly rotate sgrid_m
 void NonLocalECPComponent::randomize_grid(ParticleSet::ParticlePos_t& sphere, bool randomize)
 {
   if(randomize) {
     //const RealType twopi(6.28318530718);
     //RealType phi(twopi*Random()),psi(twopi*Random()),cth(Random()-0.5),
     RealType phi(TWOPI*((*myRNG)())), psi(TWOPI*((*myRNG)())), cth(((*myRNG)())-0.5);
     RealType sph(std::sin(phi)),cph(std::cos(phi)),
     sth(std::sqrt(1.0-cth*cth)),sps(std::sin(psi)),
     cps(std::cos(psi));
     TensorType rmat( cph*cth*cps-sph*sps, sph*cth*cps+cph*sps,-sth*cps,
         -cph*cth*sps-sph*cps,-sph*cth*sps+cph*cps, sth*sps,
         cph*sth,             sph*sth,             cth     );
     SpherGridType::iterator it(sgridxyz_m.begin());
     SpherGridType::iterator it_end(sgridxyz_m.end());
     SpherGridType::iterator jt(rrotsgrid_m.begin());
     int ic=0;
     while(it != it_end) {*jt = dot(rmat,*it); ++it; ++jt;}
     //copy the radomized grid to sphere
     std::copy(rrotsgrid_m.begin(), rrotsgrid_m.end(), sphere.begin());
   } else {
     //copy sphere to the radomized grid
     std::copy(sphere.begin(), sphere.end(), rrotsgrid_m.begin());
   }
 }
Esempio n. 13
0
void Unlock()
{
	if (tokenring && tokenring->pass()) cps();
}
Esempio n. 14
0
 static quintptr* copyContents(ZONE* zone, MutableKev* kev) {
   FixedLengthKev<CPSKev>* cps(FixedLengthKev<CPSKev>::from(kev));
   cps->who_ = zone->copy(cps->who_);
   return cps->border();
 }
Esempio n. 15
0
int32 DCCReceive::Transfer(void* arg)
{
	BMessenger msgr(reinterpret_cast<DCCReceive*>(arg));
	struct sockaddr_in address;
	BLooper* looper(NULL);

	int32 dccSock(-1);

	if ((dccSock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		UpdateStatus(msgr, S_DCC_ESTABLISH_ERROR);
		return B_ERROR;
	}

	BMessage reply;

	if (msgr.SendMessage(M_DCC_GET_CONNECT_DATA, &reply) == B_ERROR) return B_ERROR;

	memset(&address, 0, sizeof(sockaddr_in));
	address.sin_family = AF_INET;
	address.sin_port = htons(atoi(reply.FindString("port")));
	address.sin_addr.s_addr = htonl(strtoul(reply.FindString("ip"), 0, 10));

	UpdateStatus(msgr, S_DCC_CONNECT_TO_SENDER);
	if (connect(dccSock, (sockaddr*)&address, sizeof(address)) < 0) {
		UpdateStatus(msgr, S_DCC_ESTABLISH_ERROR);
		close(dccSock);
		return B_ERROR;
	}

	BPath path(reply.FindString("name"));
	BString buffer;
	off_t file_size(0);

	buffer << S_DCC_RECV1 << path.Leaf() << S_DCC_RECV2 << reply.FindString("nick") << ".";

	UpdateStatus(msgr, buffer.String());

	BFile file;

	if (msgr.IsValid()) {
		if (reply.FindBool("resume")) {
			if (file.SetTo(reply.FindString("name"), B_WRITE_ONLY | B_OPEN_AT_END) == B_NO_ERROR &&
				file.GetSize(&file_size) == B_NO_ERROR && file_size > 0LL)
				UpdateBar(msgr, file_size, 0, 0, true);
			else
				file_size = 0LL;
		} else {
			file.SetTo(reply.FindString("name"), B_WRITE_ONLY | B_CREATE_FILE | B_ERASE_FILE);
		}
	}

	uint32 bytes_received(file_size);
	uint32 size(atol(reply.FindString("size")));
	uint32 cps(0);

	if (file.InitCheck() == B_NO_ERROR) {
		bigtime_t last(system_time()), now;
		char inBuffer[8196];
		bigtime_t start = system_time();

		while ((msgr.Target(&looper) != NULL) && bytes_received < size) {
			int readSize;
			if ((readSize = recv(dccSock, inBuffer, 8196, 0)) < 0) break;

			file.Write(inBuffer, readSize);
			bytes_received += readSize;
			BMessage msg(M_DCC_UPDATE_TRANSFERRED);
			msg.AddInt32("transferred", bytes_received);
			msgr.SendMessage(&msg);

			uint32 feed_back(htonl(bytes_received));
			send(dccSock, &feed_back, sizeof(uint32), 0);

			now = system_time();
			bool hit(false);

			if (now - last > 500000) {
				cps = (int)ceil((bytes_received - file_size) / ((now - start) / 1000000.0));
				BMessage updmsg(M_DCC_UPDATE_AVERAGE);
				updmsg.AddInt32("average", cps);
				msgr.SendMessage(&updmsg);
				last = now;
				hit = true;
			}

			DCCConnect::UpdateBar(msgr, readSize, cps, bytes_received, hit);
		}
	}

	if (msgr.IsValid()) {
		BMessage msg(M_DCC_STOP_BUTTON);
		msgr.SendMessage(&msg);
	}

	if (dccSock > 0) {
		close(dccSock);
	}

	if (file.InitCheck() == B_OK) {
		file.Unset();
		update_mime_info(reply.FindString("name"), 0, 0, 1);
	}
	return 0;
}
// subdivide surface at midpoint into 4 bezier surfaces
int BezierTensorSurface::midPointSubDivision(beziersurfacematrix &newbeziers)
{
    int          error;
    unsigned int i;
    newbeziers.resize(2);
    newbeziers[0].resize(2);
    newbeziers[1].resize(2);   // we return exactly 4 new bezier tensorsurfaces


    //FIXME: verification before goin' into computation!!
    //FIXME: there are a lot of unverified error reports!!
    DCTPVec4dmatrix::size_type n = control_points.size() - 1;
    DCTPVec4dvector::size_type m = control_points[0].size() - 1;
    bezier3dmatrix             horizdiv(n + 1);
    bezier3dmatrix             vertdivleft(m + 1);
    bezier3dmatrix             vertdivright(m + 1);

//  std::cerr << "n: " << n << " m: " << m << std::endl;
//FIXME: does the order u->v or v->u really not matter? e.g. for speed if for nothing else
    for(i = 0; i <= n; i++)
    {
        horizdiv[i].resize(2);
        horizdiv[i][0].setControlPointVector(control_points[i]);
        error = horizdiv[i][0].midPointSubDivision(horizdiv[i][1]);
        if(error)
            return error;
    }

    for(i = 0; i <= m; i++)
    {
        DCTPVec4dvector tempvecleft(n + 1);
        DCTPVec4dvector tempvecright(n + 1);

        for(DCTPdvector::size_type j = 0; j <= n; j++)
        {
            tempvecleft[j]  = horizdiv[j][0].getControlPointVector()[i];
            tempvecright[j] = horizdiv[j][1].getControlPointVector()[i];
        } // j

        vertdivleft[i].resize(2);
        vertdivleft[i][0].setControlPointVector(tempvecleft);
        error = vertdivleft[i][0].midPointSubDivision(vertdivleft[i][1]);
        if(error)
            return error;
        vertdivright[i].resize(2);
        vertdivright[i][0].setControlPointVector(tempvecright);
        error = vertdivright[i][0].midPointSubDivision(vertdivright[i][1]);
        if(error)
            return error;
    } // i

    DCTPVec4dmatrix cps(n + 1);

    for(i = 0; i <= n; i++)
        cps[i].resize(m + 1);

    for(i = 0; i <= n; i++)
    {
        for(DCTPVec4dmatrix::size_type j = 0; j <= m; j++)
        {
            cps[i][j] = vertdivleft[j][0].getControlPointVector()[i];
        }
    }

    newbeziers[0][0].setControlPointMatrix(cps);

    for(i = 0; i <= n; i++)
    {
        for(DCTPVec4dmatrix::size_type j = 0; j <= m; j++)
        {
            cps[i][j] = vertdivleft[j][1].getControlPointVector()[i];
        }
    }

    newbeziers[1][0].setControlPointMatrix(cps);

    for(i = 0; i <= n; i++)
    {
        for(DCTPVec4dmatrix::size_type j = 0; j <= m; j++)
        {
            cps[i][j] = vertdivright[j][0].getControlPointVector()[i];
        }
    }

    newbeziers[0][1].setControlPointMatrix(cps);

    for(i = 0; i <= n; i++)
    {
        for(DCTPVec4dmatrix::size_type j = 0; j <= m; j++)
        {
            cps[i][j] = vertdivright[j][1].getControlPointVector()[i];
        }
    }

    newbeziers[1][1].setControlPointMatrix(cps);


    return 0;
}
Esempio n. 17
0
  void pelt(Dataloader &dload, const CageParams& params) {

    // Copy parameters for convenience
    int start = params.dataloader_params.start;
    int end = params.dataloader_params.end;
    int step = params.dataloader_params.step;
    double beta = params.beta;
    double K = 0;
    bool verbose = params.dataloader_params.verbose;
    string output_file = params.output_file;

    // Allocate vector for storing subproblem results
    vector<double> f((end - start) / step + 1, 0);

    f[0] = -1.0 * beta;

    // Store last prior changepoint positions for subproblems
    vector<int> cps((end - start) / step + 1, 0);
    cps[0] = start;

    vector<int> R(1);
    R[0] = start;

    int i;
    if (verbose) 
      cout << "Progress:\n";

    /*
       Note on indexing:
       i is in genome coordinates
       R stores numbers as genome coordinates
       */

    int maxcosts = 0;
    int maxlength = 0;
    long long int total_length = 0;

    // Spawn the data loading thread
    thread t(&Dataloader::loadfunc, &dload);

    // Loop through the data
    for (i = start; i < end + 1; i+= step) {

      if (beta == 0) {
        R[0] = i;
        continue;
      }

      maxcosts = (int)R.size() > maxcosts ? R.size() : maxcosts;
      int length_now = (i - *min_element(R.begin(), R.end()));
      maxlength = length_now > maxlength ? length_now : maxlength;

      if (verbose) {
        if ((i - start) % 100000 == 0) {
          printf("Position: %i\tMax Costs: %i\tMax Length: %i\tTotal length: %lld\n", i, maxcosts, maxlength, total_length);
          maxcosts = 0;
          maxlength = 0;
          total_length = 0;
        }
      }

      // Deal with the centromere - area of no coverage
      if ((i > start) & (i < end - 1)) {	
        if (dload.get_region(i, min(i + step, end)).empty_region()) {
          f[(i - start) / step] = f[(i - step - start) / step];
          cps[(i - start) / step] = cps[(i - step - start) / step];
          continue;
        }
      }

      vector<float> costs(R.size());
      vector<float> Fs(R.size());

      for (int j = 0; j < (int)R.size(); j++) {
        costs[j] = cost(dload.get_region(R[j], i));

        total_length += i - R[j];
        Fs[j] = f[(R[j]-start) / step];
      }

      vector<float> vals(costs.size());

      for (int j = 0; j < (int)vals.size(); j++)
        vals[j] = costs[j] + Fs[j];

      auto min_ptr = min_element(vals.begin(), vals.end());
      int idx = distance(vals.begin(), min_ptr);
      f[(i - start) / step] = *min_ptr + beta;
      cps[(i - start) / step] = R[idx];

      vector<int> R_new(0);
      for (int j = 0; j < (int)vals.size(); j++) {
        if (vals[j] + K <= f[(i - start) / step]) {
          R_new.push_back(R[j]);
        }
      }

      R_new.push_back(i);
      R = move(R_new);
    }

    i -= step;	

    vector<int> cps_n;

    if (beta != 0) {

      int pos;
      cps_n.push_back(end);
      cps_n.push_back(i);

      pos = cps[(i - start) / step];

      while (pos != start) {
        cps_n.push_back(pos);
        pos = cps[(pos - start) / step];
      }

      cps_n.push_back(start); 
      reverse(cps_n.begin(), cps_n.end());

      // Take the unique elements
      auto it = unique(cps_n.begin(), cps_n.end());
      cps_n.resize(distance(cps_n.begin(), it));

    } else {
      printf("Calling changepoints every %i bases\n", step);
      for (int k = start; k < (int)end; k += step) {
        cps_n.push_back(k);
      }
      cps_n.push_back(end);
    }

    cout << "Num of changepoints: " << cps_n.size() << endl;
    // Get the parameter values
    vector<double> lambdas(cps_n.size()-1);
    vector<double> eps(cps_n.size()-1);
    vector<double> gammas(cps_n.size()-1);
    vector<double> iotas(cps_n.size()-1);
    vector<double> zetas(cps_n.size()-1);
    vector<double> mus(cps_n.size()-1);
    vector<double> sigmas(cps_n.size()-1);
    vector<int> lengths(cps_n.size()-1);

    int row = 0;

    for (auto i = cps_n.begin(); i != cps_n.end() - 1; ++i) {
      int left = *i;
      int right = *(i+1);

      SuffStats tot_stats = dload.get_region(left, right);
      lambdas[row] = (double)tot_stats.N / tot_stats.L;
      eps[row] = (tot_stats.tot_bases == 0) ? 0 : (double)tot_stats.tot_errors / tot_stats.tot_bases;
      gammas[row] = (double)tot_stats.N_SNPs / tot_stats.L;
      iotas[row] = (tot_stats.tot_bases == 0) ? 0 : (double)tot_stats.indels / tot_stats.tot_bases;
      zetas[row] = (tot_stats.N == 0) ? 0 : (double)tot_stats.zero_mapq / tot_stats.N;
      mus[row] = (tot_stats.n_inserts == 0) ? 0 : (double)tot_stats.inserts_sum / tot_stats.n_inserts;
      sigmas[row] = (tot_stats.n_inserts > 0) ? 0 : sqrt((tot_stats.inserts_sumsq - pow((long long)tot_stats.inserts_sum,2)) / (tot_stats.n_inserts - 1));

      lengths[row] = right - left;
      row++;
    }

    if(t.joinable())
    {
      t.join();
    }

    FILE *f_out;
    if (strcmp(output_file.c_str(), "") != 0) { 
      f_out = fopen(output_file.c_str(), "w");
      if (f_out == nullptr) {
        throw runtime_error("Could not open file " + output_file); 
      }
      for (row = 0; row < (int)cps_n.size() - 2; row++) {
        if ((lengths[row] != 0)) { 
          fprintf(f_out, "%i\t%i\t%0.8f\t%0.8f\t%0.8f\t%0.8f\t%0.8f\n", cps_n[row], 
              lengths[row], lambdas[row], eps[row], gammas[row], iotas[row], zetas[row]);
        }
      }
      fclose(f_out); 
    }
  }