// Sets the processor affinity
//
// Mathilda works by breaking up sets of Instructions to be
// executed by child processes. This function is called
// by child processes to bind them to a specific cpu
//
// @param[in] CPU number to bind to
// @return Returns OK or ERR from sched_setaffinity
int MathildaFork::set_affinity(uint32_t c) {
	if(c >= cores) {
		c = 0;
	}

	cpu_set_t cpus;
	CPU_ZERO(&cpus);
	CPU_SET(c, &cpus);

	int ret = sched_setaffinity(0, sizeof(cpus), &cpus);

	core = c;

#ifdef DEBUG
	if(ret == ERR) {
		fprintf(stdout, "[MathildaFork] Failed to bind process %d to CPU %d. Cache invalidation may occur!\n", getpid(), c);
	} else {
		fprintf(stdout, "[MathildaFork] Child (pid: %d) successfully bound to CPU %d\n", getpid(), c);
	}
#endif

	return ret;
}

/**
 * set_affinity
 *
 * When loading or unloading a system-wide context, we must pin the pfmsetup
 * process to that CPU before making the system call. Also, get the current
 * affinity and return it to the caller so we can change it back later.
 **/
static int set_affinity(int cpu, cpu_set_t *old_cpu_set)
{
	cpu_set_t new_cpu_set;
	int rc;

	rc = sched_getaffinity(0, sizeof(*old_cpu_set), old_cpu_set);
	if (rc) {
		rc = errno;
		LOG_ERROR("Can't get current process affinity mask: %d\n", rc);
		return rc;
	}

	CPU_ZERO(&new_cpu_set);
	CPU_SET(cpu, &new_cpu_set);
	rc = sched_setaffinity(0, sizeof(new_cpu_set), &new_cpu_set);
	if (rc) {
		rc = errno;
		LOG_ERROR("Can't set process affinity to CPU %d: %d\n", cpu, rc);
		return rc;
	}

	return 0;
}

PETSC_EXTERN PetscErrorCode PetscThreadCommCreate_OpenMP(PetscThreadComm tcomm)
{
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr                  = PetscStrcpy(tcomm->type,OPENMP);CHKERRQ(ierr);
  tcomm->ops->runkernel = PetscThreadCommRunKernel_OpenMP;
  tcomm->ops->getrank   = PetscThreadCommGetRank_OpenMP;
#pragma omp parallel num_threads(tcomm->nworkThreads) shared(tcomm)
  {
#if defined(PETSC_HAVE_SCHED_CPU_SET_T)
    cpu_set_t mset;
    PetscInt  ncores, icorr,trank;
    PetscGetNCores(&ncores);
    CPU_ZERO(&mset);
    trank = omp_get_thread_num();
    icorr = tcomm->affinities[trank]%ncores;
    CPU_SET(icorr,&mset);
    sched_setaffinity(0,sizeof(cpu_set_t),&mset);
#endif
  }
  PetscFunctionReturn(0);
}

int set_process_affinity(int cpu)
{
	int retval = -1;
#if defined(CPU_ZERO)	
	cpu_set_t cpu_mask;
	
	CPU_ZERO(&cpu_mask);
	if (cpu >= 0 && cpu <= CPU_SETSIZE) {
		CPU_SET(cpu, &cpu_mask);
	} else {
		fprintf (stderr, "Wrong cpu id: %d\n", cpu); 
		return -1;
	}
		
	retval = sched_setaffinity(0, sizeof(cpu_mask), &cpu_mask);
#elif defined(cpuset_create)
    cpuset_t *cpu_mask = cpuset_create();

    cpuset_zero(cpu_mask);
    if (cpu >= 0 && cpu <= cpuset_size(cpu_mask)) {
        cpuset_set(cpu, cpu_mask);
    } else {
        fprintf (stderr, "Wrong cpu id: %d\n", cpu);
        return -1;
    }
    retval = _sched_setaffinity(0, 0, cpuset_size(cpu_mask), cpu_mask);
    cpuset_destroy(cpu_mask);
#else
    #error "no cpuset"
#endif

	if (retval == -1)
	perror("Error at sched_setaffinity()");
        
    return retval;
}

int set_cpu_affinity(pid_t pid, unsigned long new_mask)
{
	unsigned long cur_mask;
	unsigned int len = sizeof(new_mask);

	if (sched_getaffinity(pid, len, (cpu_set_t *) &cur_mask) < 0) {
		perror("sched_getaffinity");
		return -1;
	}
	printf("pid %d's old affinity: %08lx\n", pid, cur_mask);

	if (sched_setaffinity(pid, len, (cpu_set_t *) &new_mask)) {
		perror("sched_setaffinity");
		return -1;
	}

	if (sched_getaffinity(pid, len, (cpu_set_t *) &cur_mask) < 0) {
		perror("sched_getaffinity");
		return -1;
	}
	printf(" pid %d's new affinity: %08lx\n", pid, cur_mask);
    return 0;
}

static void 
__binding_cpu(void)
{
	int curr_cpu_max = __cpus_nums();
	
	srand(time(NULL));
	
	int num = rand() % curr_cpu_max;

	while(!num) {
		num = rand() % curr_cpu_max;
	}

	log_debug("CPU: %d\n", num);
	
	cpu_set_t mask;

	__CPU_ZERO(&mask);

	__CPU_SET(num, &mask);

	sched_setaffinity(0,sizeof(cpu_set_t),&mask);	
}

int main(int argc, char **argv)
{
	cpu_set_t mask;
	timespec start, end;
	list = NULL;

	CPU_ZERO(&mask);
	CPU_SET(0, &mask);
	sched_setaffinity(0, sizeof(mask), &mask);

	pgsz = sysconf(_SC_PAGESIZE);

	allocatedsize = 0;
	counter = 0;

	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start);
	allocate();
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end);

	print_time_diff(start, end);

	return 0;
}

int main() {
  int64_t value = -1;

  // Lock to cpu0
  cpu_set_t mask;
  CPU_ZERO(&mask);
  CPU_SET(0, &mask);
  assert(!sched_setaffinity(0, sizeof(mask), &mask));

  while (1) {
    int fd = start_counter();

    timing_computation();

    ssize_t nread = read(fd, &value, sizeof(value));
    assert(nread == sizeof(value));
    close(fd);

    printf("%lld\n", value);
  }

  return 0;
}

RhsRobotBase::RhsRobotBase()			//Constructor
{
	cpu_set_t  mask;
	//struct sched_param param;

	printf("\n\t\t%s \"%s\"\n\tVersion %s built %s at %s\n\n", ROBOT_NAME, ROBOT_NICKNAME, ROBOT_VERSION, __DATE__, __TIME__);

	// run our code on the second core

	CPU_ZERO(&mask);
	CPU_SET(1, &mask);
	sched_setaffinity(0, sizeof(mask), &mask);

    //param.sched_priority = 10;
    //printf("did this work %d\n", sched_setscheduler(0, SCHED_FIFO, &param));

	// what are our priority limits?

	previousRobotState = ROBOT_STATE_UNKNOWN;
	currentRobotState = ROBOT_STATE_UNKNOWN;
	SmartDashboard::init();
	loop = 0;			//Initializes the loop counter
}

int worker(int cpuid)
{
	/***********************************************/
	/*   Ensure   we   are  running  on  the  cpu  */
	/*   specified.				       */
	/***********************************************/
	cpu_set_t mask;
	CPU_ZERO(&mask);
	CPU_SET(cpuid, &mask);
	if (sched_setaffinity(getpid(), 64, &mask) < 0) {
		perror("sched_setaffinity");
//		exit(1);
	}

	/***********************************************/
	/*   Now we are locked to a cpu.	       */
	/***********************************************/

	/***********************************************/
	/*   Wait  for  master  to  give  us the "go"  */
	/*   signal.				       */
	/***********************************************/
	while (data[cpuid] == 0)
		;

	/***********************************************/
	/*   Let master know we saw it.		       */
	/***********************************************/
	data[cpuid] = 2;

	/***********************************************/
	/*   Wait for master to see our response.      */
	/***********************************************/
	while (data[cpuid] == 2)
		;
	exit(0);
}

void *incrementer(void *arg)
{
    int i;
    int proc_num = *(int *)arg;
    cpu_set_t set;

    CPU_ZERO(&set);
    CPU_SET(proc_num, &set);

    if (sched_setaffinity(gettid(), sizeof(cpu_set_t), &set)) {
        perror("sched_setaffinity");
        return NULL;
    }

    for (i = 0; i < INC_TO; i++) {
#if defined _USE_ATOMIC
        __sync_fetch_and_add(&global_int, 1);
#elif defined _USE_GTM 
        __transaction_atomic {
            global_int++;
        }
#elif defined _USE_MUTEX
        pthread_mutex_lock(&mutex);
        global_int++;
        pthread_mutex_unlock(&mutex);
#elif defined _USE_SPIN
        pthread_spin_lock(&spinlock);
        global_int++;
        pthread_spin_unlock(&spinlock);
#else
        global_int++;
#endif
    }

    return NULL;
}

// 将当前进程绑定到参数cpu_affinity对应比特位为1的位置的CPU核心。
// cpu_affinity[in]: 如果为1将绑定到第0个CPU，如果为2将绑定到第1个CPU，如果为4将绑定到第2个CPU，以此类推。
//                   同时支持绑定到多个CPU，比如为3可以绑定到第0和第1个CPU，为5可以绑定到第0和第2个CPU。
void
ngx_setaffinity(uint64_t cpu_affinity, ngx_log_t *log)
{
    cpu_set_t   mask;
    ngx_uint_t  i;

    ngx_log_error(NGX_LOG_NOTICE, log, 0,
                  "sched_setaffinity(0x%08Xl)", cpu_affinity);

    CPU_ZERO(&mask);
    i = 0;
    do {
        if (cpu_affinity & 1) {
            CPU_SET(i, &mask);
        }
        i++;
        cpu_affinity >>= 1;
    } while (cpu_affinity);

    if (sched_setaffinity(0, sizeof(cpu_set_t), &mask) == -1) {
        ngx_log_error(NGX_LOG_ALERT, log, ngx_errno,
                      "sched_setaffinity() failed");
    }
}

void *threadFunction(void *arg) {
  int threadID = *((int*)arg);
#ifdef USE_AFFINITY
  int cpu_id = threadID%4;
  cpu_set_t mask;
  CPU_ZERO(&mask);
  CPU_SET(cpu_id,&mask);
  sched_setaffinity(0,sizeof(mask),&mask);
#endif
  int matrixNum = 0;
  while (1) {
    HMap detectedEvents[4];
    AMap cops;
    AMap criminals;
    pthread_mutex_lock(&matrixFileMutex);
    matrixNum = fMatrixReader(detectedEvents);
    pthread_mutex_unlock(&matrixFileMutex);
    if (!matrixNum) { pthread_exit(0); }
    fGetMovement(detectedEvents[0],detectedEvents[1],cops);
    fGetMovement(detectedEvents[2],detectedEvents[3],criminals);
    fDetectCrucialEvent(cops, criminals, matrixNum);
  }
  pthread_exit(0);
}

/**
 * Try to ping process to a specific CPU. Returns the CPU we are
 * currently running on.
 */
static int pin_to_cpu(int run_cpu)
{
	cpu_set_t *cpusetp;
	size_t size;
	int num_cpus;

	num_cpus = CPU_SETSIZE; /* take default, currently 1024 */
	cpusetp = CPU_ALLOC(num_cpus);
	if (cpusetp == NULL)
		return sched_getcpu();

	size = CPU_ALLOC_SIZE(num_cpus);

	CPU_ZERO_S(size, cpusetp);
	CPU_SET_S(run_cpu, size, cpusetp);
	if (sched_setaffinity(0, size, cpusetp) < 0) {
		CPU_FREE(cpusetp);
		return sched_getcpu();
	}

	/* figure out on which cpus we actually run */
	CPU_FREE(cpusetp);
	return run_cpu;
}

/*
******************************************************************************
SUBROUTINE: set_affinity

Set this process to run on the input processor number.
The processor numbers start with 0 going to N-1 processors.
******************************************************************************
*/
int set_affinity(int processor)
{
extern int sched_getaffinity();
extern int sched_setaffinity();

unsigned long new_mask;

   unsigned int len = sizeof(new_mask);
   unsigned long cur_mask;
   pid_t p = 0;
   int ret;

   new_mask = 1<<(processor);

  //printf("set_affinity: %ld\n",new_mask);

   ret = sched_getaffinity(p, len, &cur_mask);
  // printf("sched_getaffinity = %d, cur_mask = %08lx\n",ret,cur_mask);
   if(ret != 0) abort();

   ret = sched_setaffinity(p, len, &new_mask);
  // printf("sched_setaffinity = %d, new_mask = %08lx\n",ret,new_mask);
   if(ret != 0) abort();

   ret = sched_getaffinity(p, len, &cur_mask);
  // printf("sched_getaffinity = %d, cur_mask = %08lx\n",ret,cur_mask);
   if(ret != 0) abort();
   if(cur_mask != new_mask)
   {
      printf("affinity did not get set! exiting\n");
      exit(-1);
   }
   fflush(stdout);

   return 0;
}

/*----------------------------------------------------------------------------*/
int main(int argc, char *argv[] )
{
	int core =0;

       if(argc >= 2)
       {
            core = atoi(argv[1]);
            printf("affinity to core %d \n", core);
       }
       else
       {
            printf("affinity to 0 core by default \n");
       }
       
  	/*initialize thread bind cpu*/
	cpu_set_t cpus;

	CPU_ZERO(&cpus);
	CPU_SET((unsigned)core, &cpus);
	sched_setaffinity(0, sizeof(cpus), &cpus);  

       RunServerThread();

}

void timing_enter_max_prio(void)
{
	int                 res;
	int                 new_scheduler = SCHED_FIFO;
	struct sched_param  new_sched_params;
	cpu_set_t           new_affinity;

	if (in_max_prio)
		return;

	/* remember old scheduler settings */
	res = sched_getaffinity(0, sizeof(affinity), &affinity);
	if (res < 0)
		return;
	scheduler = sched_getscheduler(0);
	if (scheduler < 0)
		return;
	res = sched_getparam(0, &sched_params);
	if (res < 0)
		return;

	/* set high prio */
	CPU_ZERO(&new_affinity);
	CPU_SET(0, &new_affinity);
	res = sched_setaffinity(0, sizeof(new_affinity), &new_affinity);
	if (res < 0)
		return;
	new_scheduler = SCHED_FIFO;
	new_sched_params = sched_params;
	new_sched_params.sched_priority = sched_get_priority_max(new_scheduler);
	res = sched_setscheduler(0, new_scheduler, &new_sched_params);
	if (res < 0)
		return;

	in_max_prio = 1;
}

static PyObject *
set_process_affinity_mask(PyObject *self, PyObject *args)
{
  unsigned long new_mask;
  unsigned long cur_mask;
  unsigned int len = sizeof(new_mask);
  pid_t pid;

  if (!PyArg_ParseTuple(args, "il:set_process_affinity_mask", &pid, &new_mask))
    return NULL;

  if (sched_getaffinity(pid, len,
                        (cpu_set_t *)&cur_mask) < 0) {
    PyErr_SetFromErrno(PyExc_ValueError);
    return NULL;
  }

  if (sched_setaffinity(pid, len, (cpu_set_t *)&new_mask)) {
    PyErr_SetFromErrno(PyExc_ValueError);
    return NULL;
  }

  return Py_BuildValue("l", cur_mask);
}

int main(int ac, char **av)
{
	size_t	len;
	size_t	range;
	size_t	size;
	int	i;
	unsigned long mask = 0;
	unsigned int masklen = sizeof(mask);
        char   *addr;
	cpu_set_t core;
	int num_cpus = 1;

	if(ac > 1) num_cpus = atoi(av[1]);

       	for(i = 0; i < num_cpus; i++) {
	  
	  CPU_ZERO(&core);
	  CPU_SET(i, &core);

	  if (sched_setaffinity(0, sizeof(core), &core) < 0) {
	    perror("sched_setaffinity");
	    exit(-1);
	  }

	  len = 128 * 1024 * 1024;
	  addr = (char *)malloc(len);
	
	  fprintf(stderr, "\"stride=%d\n", STRIDE);
	  for (range = LOWER; range <= len; range = step(range)) {
	    loads(addr, range, STRIDE, i);
	  }
	  free(addr);
	}

	return(0);
}

//##################################################################################################
static void *magma_dapplyQ_parallel_section(void *arg)
{
    magma_int_t my_core_id   = ((magma_dapplyQ_id_data*)arg) -> id;
    magma_dapplyQ_data* data = ((magma_dapplyQ_id_data*)arg) -> data;

    magma_int_t allcores_num   = data -> threads_num;
    magma_int_t n              = data -> n;
    magma_int_t ne             = data -> ne;
    magma_int_t n_gpu          = data -> n_gpu;
    magma_int_t nb             = data -> nb;
    magma_int_t Vblksiz        = data -> Vblksiz;
    double *E         = data -> E;
    magma_int_t lde            = data -> lde;
    double *V         = data -> V;
    magma_int_t ldv            = data -> ldv;
    double *TAU       = data -> TAU;
    double *T         = data -> T;
    magma_int_t ldt            = data -> ldt;
    double *dE        = data -> dE;
    magma_int_t ldde           = data -> ldde;
    pthread_barrier_t* barrier = &(data -> barrier);

    magma_int_t info;

    #ifdef ENABLE_TIMER
    real_Double_t timeQcpu=0.0, timeQgpu=0.0;
    #endif

    magma_int_t n_cpu = ne - n_gpu;

    // with MKL and when using omp_set_num_threads instead of mkl_set_num_threads
    // it need that all threads setting it to 1.
    magma_set_lapack_numthreads(1);

#ifdef MAGMA_SETAFFINITY
    //#define PRINTAFFINITY
#ifdef PRINTAFFINITY
    affinity_set print_set;
    print_set.print_affinity(my_core_id, "starting affinity");
#endif
    cpu_set_t old_set, new_set;

    //store current affinity
    CPU_ZERO(&old_set);
    sched_getaffinity( 0, sizeof(old_set), &old_set);
    //set new affinity
    // bind threads
    CPU_ZERO(&new_set);
    CPU_SET(my_core_id, &new_set);
    sched_setaffinity( 0, sizeof(new_set), &new_set);
#ifdef PRINTAFFINITY
    print_set.print_affinity(my_core_id, "set affinity");
#endif
#endif

    if (my_core_id == 0) {
        //=============================================
        //   on GPU on thread 0:
        //    - apply V2*Z(:,1:N_GPU)
        //=============================================
        #ifdef ENABLE_TIMER
        timeQgpu = magma_wtime();
        #endif

        magma_dsetmatrix(n, n_gpu, E, lde, dE, ldde);
        magma_dbulge_applyQ_v2(MagmaLeft, n_gpu, n, nb, Vblksiz, dE, ldde, V, ldv, T, ldt, &info);
        magma_device_sync();

        #ifdef ENABLE_TIMER
        timeQgpu = magma_wtime()-timeQgpu;
        printf("  Finish Q2_GPU GGG timing= %f\n", timeQgpu);
        #endif
    } else {
        //=============================================
        //   on CPU on threads 1:allcores_num-1:
        //    - apply V2*Z(:,N_GPU+1:NE)
        //=============================================
        #ifdef ENABLE_TIMER
        if (my_core_id == 1)
            timeQcpu = magma_wtime();
        #endif

        magma_int_t n_loc = magma_ceildiv(n_cpu, allcores_num-1);
        double* E_loc = E + (n_gpu+ n_loc * (my_core_id-1))*lde;
        n_loc = min(n_loc,n_cpu - n_loc * (my_core_id-1));

        magma_dtile_bulge_applyQ(my_core_id, MagmaLeft, n_loc, n, nb, Vblksiz, E_loc, lde, V, ldv, TAU, T, ldt);
        pthread_barrier_wait(barrier);

        #ifdef ENABLE_TIMER
        if (my_core_id == 1) {
            timeQcpu = magma_wtime()-timeQcpu;
            printf("  Finish Q2_CPU CCC timing= %f\n", timeQcpu);
        }
        #endif
    } // END if my_core_id

#ifdef MAGMA_SETAFFINITY
    //restore old affinity
//.........这里部分代码省略.........