/* classic Sedgewick ternary sort, tweaked a bit for speed */
INLINE static
void TernarySort(uint16 **lowerb_t,
uint16 **upperb_t,
uint32 depth_t)
{
sint32 sp = 1;
uint32 v, depth;
uint16 **lb, **ub, **i, **j, **lowerb, **upperb;
lowerb = lowerb_t;
upperb = upperb_t;
depth = depth_t;
while (sp > 0)
{
{
sint32 r = upperb - lowerb + 1;
if (r <= MAX_INSERTSORT)
{
if (r >= 2) ShellSort(lowerb, upperb, depth);
POP(lowerb, upperb, depth);
continue;
}
if (r > 64)
{ /* median-of-3 median-of-3 */
uint v1, v2, v3;
r >>= 3;
lb = lowerb;
{
uint v_1, v_2, v_3;
v_1 = KEY(lb); v_2 = KEY(lb+=r); v_3 = KEY(lb+=r);
v1 = MEDIAN(v_1, v_2, v_3);
}
{
uint v_1, v_2, v_3;
v_1 = KEY(lb+=r); v_2 = KEY(lb+=r); v_3 = KEY(lb+=r);
v2 = MEDIAN(v_1, v_2, v_3);
}
{
uint v_1, v_2, v_3;
v_1 = KEY(lb+=r); v_2 = KEY(lb+=r); v_3 = KEY(upperb);
v3 = MEDIAN(v_1, v_2, v_3);
}
v = MEDIAN(v1, v2, v3);
}
else
{ /* median-of-3 */
uint v1, v2, v3;
v1 = KEY(lowerb);
v2 = KEY(lowerb + (r >> 1));
v3 = KEY(upperb);
v = MEDIAN(v1, v2, v3);
}
}
Void MbDataAccess::xGetMvPredictor( Mv& rcMvPred, SChar scRef, ListIdx eListIdx, PredictionType ePredType, LumaIdx cIdx, LumaIdx cIdxEnd )
{
const Bool bCurrentFieldFlag = m_rcMbCurr.getFieldFlag();
//===== set motion vector predictors: A, B, C =====
B4x4Idx cIdxA = cIdx .b4x4();
B4x4Idx cIdxB = cIdx .b4x4();
B4x4Idx cIdxD = cIdx .b4x4();
B4x4Idx cIdxC = cIdxEnd.b4x4();
const MbData& rcMbDataA = xGetBlockLeft ( cIdxA );
const MbData& rcMbDataB = xGetBlockAbove ( cIdxB );
const MbData& rcMbDataC = xGetBlockAboveRight( cIdxC );
rcMbDataA.getMbMotionData( eListIdx ).getMv3DNeighbour( m_cMv3D_A, cIdxA, bCurrentFieldFlag );
rcMbDataB.getMbMotionData( eListIdx ).getMv3DNeighbour( m_cMv3D_B, cIdxB, bCurrentFieldFlag );
rcMbDataC.getMbMotionData( eListIdx ).getMv3DNeighbour( m_cMv3D_C, cIdxC, bCurrentFieldFlag );
if( m_cMv3D_C == BLOCK_NOT_AVAILABLE )
{
const MbData& rcMbDataD = xGetBlockAboveLeft ( cIdxD );
rcMbDataD.getMbMotionData( eListIdx ).getMv3DNeighbour( m_cMv3D_C, cIdxD, bCurrentFieldFlag );
}
//===== check directional prediction types =====
if( ( ePredType == PRED_A && m_cMv3D_A == scRef ) ||
( m_cMv3D_A == scRef && m_cMv3D_B != scRef && m_cMv3D_C != scRef ) ||
( m_cMv3D_B == BLOCK_NOT_AVAILABLE && m_cMv3D_C == BLOCK_NOT_AVAILABLE ) )
{
rcMvPred = m_cMv3D_A;
return;
}
if( ( ePredType == PRED_B && m_cMv3D_B == scRef ) ||
( m_cMv3D_A != scRef && m_cMv3D_B == scRef && m_cMv3D_C != scRef ) )
{
rcMvPred = m_cMv3D_B;
return;
}
if( ( ePredType == PRED_C && m_cMv3D_C == scRef ) ||
( m_cMv3D_A != scRef && m_cMv3D_B != scRef && m_cMv3D_C == scRef ) )
{
rcMvPred = m_cMv3D_C;
return;
}
#define MEDIAN(a,b,c) ((a)>(b)?(a)>(c)?(b)>(c)?(b):(c):(a):(b)>(c)?(a)>(c)?(a):(c):(b))
{
rcMvPred.setHor( MEDIAN( m_cMv3D_A.getHor(), m_cMv3D_B.getHor(), m_cMv3D_C.getHor() ) );
rcMvPred.setVer( MEDIAN( m_cMv3D_A.getVer(), m_cMv3D_B.getVer(), m_cMv3D_C.getVer() ) );
}
#undef MEDIAN
}
void median3_func( int num , double to,double dt, float *vec )
{
int ii ;
float aa,bb,cc ;
bb = vec[0] ; cc = vec[1] ;
for( ii=1 ; ii < num-1 ; ii++ ){
aa = bb ; bb = cc ; cc = vec[ii+1] ;
vec[ii] = MEDIAN(aa,bb,cc) ; /* see mrilib.h */
}
return ;
}
int main(void)
{
int x, y, n;
x = 2;
y = 6;
n = 21;
printf("%d\n", CHECK(x, y, n));
printf("%d\n", MEDIAN(x, y, n));
printf("%d\n", POLYNOMIAL(x));
return 0;
}
void arc(int radius, int degree) {
long l = 0, r = 0, avg = 0;//, ol = 0, ort = 0;
long long break_val = 0;
PresetEncoderCounts(0,0);
break_val = (epr * ABS(radius));
UARTprintf("Rad: %3d %d\n",ABS(radius),break_val);
//if (ABS(radius) < BOT_WIDTH/2)
// return;
while(break_val > WHEEL_RAD * avg * 180 * degree)
{
GetEncoderCounts(&l,&r);
//pid(radius+BOT_WIDTH,radius-BOT_WIDTH);
//pid(radius,BOT_WIDTH);
SetMotorPowers(MEDIAN(0,127,127 * (radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2)),
MEDIAN(0,127,127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2)));
avg = (l+r)/2;
UARTprintf("Compare: %10d %10d Powers2: %5d %5d\r",break_val, WHEEL_RAD * avg * degree,
127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2),127*(radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2));
//UARTprintf("Average: %10d %10d\r",(radius + BOT_WIDTH>>1),(radius - BOT_WIDTH>>1));
//ol = l; ort = r;
}
SetMotorPowers(0,0);
}
/*
Similar to the algorithm used by glibc, but glibc uses an explict stack
while we use recursion, since I'm lazy.
*/
static inline int qsort_partition(void **arr, int left, int right,
cmp_fun cmp){
int pivot_idx = MEDIAN(left, right, (left+right)/2);
//int pivot_idx = left;
void* pivot = arr[pivot_idx];
int i = left - 1, j = right + 1;
do {
do {
i++;
} while(cmp(arr[i], pivot));
do {
j--;
} while(cmp(pivot, arr[j]));
if(i<j){
SWAP(arr[i], arr[j]);
} else {
return j;
}
} while(i<=j);
return j;
}
/*
A simple version of quicksort; taken from Kernighan and Ritchie, page 87.
Assumes 0 origin for v, number of elements = right+1 (right is index of
right-most member).
*/
static void PetscSortInt_Private(PetscInt *v,PetscInt right)
{
PetscInt i,j,pivot,tmp;
if (right <= 1) {
if (right == 1) {
if (v[0] > v[1]) SWAP(v[0],v[1],tmp);
}
return;
}
i = MEDIAN(v,right); /* Choose a pivot */
SWAP(v[0],v[i],tmp); /* Move it out of the way */
pivot = v[0];
for (i=0,j=right+1;; ) {
while (++i < j && v[i] <= pivot) ; /* Scan from the left */
while (v[--j] > pivot) ; /* Scan from the right */
if (i >= j) break;
SWAP(v[i],v[j],tmp);
}
SWAP(v[0],v[j],tmp); /* Put pivot back in place. */
PetscSortInt_Private(v,j-1);
PetscSortInt_Private(v+j+1,right-(j+1));
}
void pid(signed long e0, signed long e1) {
int error = (e1-e0);
SetMotorPowers(MEDIAN(-80,80,64-error/4),
MEDIAN(-80,80,64+error/4));
}
void assemble_contigs_stats_print(const AssembleContigStats *s)
{
ctx_assert(s->lengths.len == s->junctns.len);
ctx_assert(s->lengths.len == s->num_contigs);
size_t i, ncontigs = s->num_contigs;
if(ncontigs == 0) {
status("[asm] No contigs assembled");
return;
}
qsort(s->lengths.b, ncontigs, sizeof(s->lengths.b[0]), cmp_size);
qsort(s->junctns.b, ncontigs, sizeof(s->junctns.b[0]), cmp_size);
size_t len_n50, jnc_n50;
size_t len_median, jnc_median, len_mean, jnc_mean;
size_t len_min, len_max, jnc_min, jnc_max;
// Calculate N50s
len_n50 = calc_N50(s->lengths.b, ncontigs, s->total_len);
jnc_n50 = calc_N50(s->junctns.b, ncontigs, s->total_junc);
// Calculate medians, means
len_median = MEDIAN(s->lengths.b, ncontigs);
jnc_median = MEDIAN(s->junctns.b, ncontigs);
len_mean = (double)s->total_len / ncontigs;
jnc_mean = (double)s->total_junc / ncontigs;
// Calculate min, max
len_min = s->lengths.b[0];
jnc_min = s->junctns.b[0];
len_max = s->lengths.b[ncontigs-1];
jnc_max = s->junctns.b[ncontigs-1];
// Print number of contigs
char num_contigs_str[50], reseed_str[50], seed_not_fnd_str[50];
char seed_kmers_str[50], seed_paths_str[50];
long_to_str(ncontigs, num_contigs_str);
long_to_str(s->num_reseed_abort, reseed_str);
long_to_str(s->num_seeds_not_found, seed_not_fnd_str);
long_to_str(s->num_contigs_from_seed_kmers, seed_kmers_str);
long_to_str(s->num_contigs_from_seed_paths, seed_paths_str);
status(PREFIX"pulled out %s contigs, %s from seed kmers, %s from seed paths",
num_contigs_str, seed_kmers_str, seed_paths_str);
status(PREFIX"no-reseed aborted %s times", reseed_str);
status(PREFIX"seed kmer not found %s times", seed_not_fnd_str);
char len_min_str[50], len_max_str[50], len_total_str[50];
char len_mean_str[50], len_median_str[50], len_n50_str[50];
char jnc_min_str[50], jnc_max_str[50], jnc_total_str[50];
char jnc_mean_str[50], jnc_median_str[50], jnc_n50_str[50];
// Use ulong_to_str instead of num_to_str to get better accuracy
// e.g. 966 instead of 1K
ulong_to_str(len_mean, len_mean_str);
ulong_to_str(jnc_mean, jnc_mean_str);
ulong_to_str(len_median, len_median_str);
ulong_to_str(jnc_median, jnc_median_str);
ulong_to_str(len_n50, len_n50_str);
ulong_to_str(jnc_n50, jnc_n50_str);
ulong_to_str(len_min, len_min_str);
ulong_to_str(jnc_min, jnc_min_str);
ulong_to_str(len_max, len_max_str);
ulong_to_str(jnc_max, jnc_max_str);
ulong_to_str(s->total_len, len_total_str);
ulong_to_str(s->total_junc, jnc_total_str);
status(PREFIX"Lengths: mean: %s median: %s N50: %s min: %s max: %s total: %s [kmers]",
len_mean_str, len_median_str, len_n50_str, len_min_str, len_max_str, len_total_str);
status(PREFIX"Junctions: mean: %s median: %s N50: %s min: %s max: %s total: %s [out >1]",
jnc_mean_str, jnc_median_str, jnc_n50_str, jnc_min_str, jnc_max_str, jnc_total_str);
status(PREFIX"Max junction density: %.2f\n", s->max_junc_density);
timestamp();
message(PREFIX" Outdegree: ");
char nout_str[50];
for(i = 0; i <= 4; i++) {
message("\t%zu:%s [%zu%%]", i, ulong_to_str(s->contigs_outdegree[i], nout_str),
(size_t)((100.0*s->contigs_outdegree[i])/(2.0*ncontigs)+0.5));
}
message("\n");
_print_path_dist(s->paths_held, AC_MAX_PATHS, "Paths held", ncontigs);
_print_path_dist(s->paths_cntr, AC_MAX_PATHS, "Paths counter", ncontigs);
const uint64_t *states = s->grphwlk_steps;
size_t nsteps = s->total_len - s->num_contigs, ncontigends = 2*s->num_contigs;
status(PREFIX"Traversal succeeded because:");
_print_grphwlk_state("Pop straight ......... ", states[GRPHWLK_POPFWD], nsteps);
_print_grphwlk_state("Col straight ......... ", states[GRPHWLK_COLFWD], nsteps);
_print_grphwlk_state("PopFork use colour ... ", states[GRPHWLK_POPFRK_COLFWD],nsteps);
_print_grphwlk_state("Go paths ............. ", states[GRPHWLK_USEPATH], nsteps);
const uint64_t *stops = s->stop_causes;
status(PREFIX"Traversal halted because:");
_print_grphwlk_state("No coverage .......... ", stops[ASSEM_STOP_NOCOVG], ncontigends);
_print_grphwlk_state("No colour covg ....... ", stops[ASSEM_STOP_NOCOLCOVG], ncontigends);
_print_grphwlk_state("No paths ............. ", stops[ASSEM_STOP_NOPATHS], ncontigends);
_print_grphwlk_state("Paths split .......... ", stops[ASSEM_STOP_SPLIT_PATHS], ncontigends);
_print_grphwlk_state("Missing paths ........ ", stops[ASSEM_STOP_MISSING_PATHS], ncontigends);
_print_grphwlk_state("Graph cycles ......... ", stops[ASSEM_STOP_CYCLE], ncontigends);
_print_grphwlk_state("Low step confidence .. ", stops[ASSEM_STOP_LOW_STEP_CONF], ncontigends);
_print_grphwlk_state("Low cumul. confidence ", stops[ASSEM_STOP_LOW_CUMUL_CONF],ncontigends);
size_t njunc = states[GRPHWLK_USEPATH] +
stops[ASSEM_STOP_NOPATHS] +
stops[ASSEM_STOP_SPLIT_PATHS] +
stops[ASSEM_STOP_MISSING_PATHS];
ctx_assert2(s->total_junc == states[GRPHWLK_USEPATH], "%zu vs %zu",
(size_t)s->total_junc, (size_t)states[GRPHWLK_USEPATH]);
status(PREFIX"Junctions:");
_print_grphwlk_state("Paths resolved", states[GRPHWLK_USEPATH], njunc);
}
//------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------
void Calculate_Baseline( std::vector<double> DIAM,std::vector<double> ARC, std::vector<double> &baseline, std::vector<double> &weights ){
baseline.clear();
weights.clear();
unsigned int Npoints = ARC.size();
baseline.resize(Npoints, 0.0);
weights.resize(Npoints, 0.0);
std::vector<double> r_mad (Npoints, 0.0);
std::vector<double> DIAM_noise (Npoints, 0.0);
double MIN_, MAX_;
//REMOVE OUTLIERS USING MAD (Median Absolute Deviation)
double outlier_cutoff = 3.0;
double median_D, mad;
median_D = MEDIAN(DIAM);
for(unsigned i = 0; i < Npoints; i = i + 1) {
r_mad[i] = abs( DIAM[i] - median_D );
}
mad = MEDIAN(r_mad);
// use fitting weights to exclude the outliers
if( mad > 1.0e-6 ) {
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
r_mad[i] = abs( DIAM[i] - median_D ) / mad;
if( r_mad[i] > outlier_cutoff ) {
weights[i] = 0.0;
} else {
weights[i] = 1.0;
}
}
} else {
// will end up here if more than 50% of data points had the same value
// add 1% noise to DIAM profile and recalculate mad
std::vector<double> DIAM_noise = DIAM;
MIN_ = MINIMUM(DIAM);
for (unsigned i=0; i < Npoints; i = i + 1 ) {
DIAM_noise[i] = DIAM[i] * 0.01 * MIN_ * RandGen((double)-1.0,(double)1.0);
}
median_D = MEDIAN(DIAM_noise);
for(unsigned i = 0; i < Npoints; i = i + 1) {
r_mad[i] = abs( DIAM_noise[i] - median_D );
}
mad = MEDIAN(r_mad);
if( mad > 1.0e-6 ) {
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
r_mad[i] = abs( DIAM_noise[i] - median_D ) / mad;
if( r_mad[i] > outlier_cutoff ) {
weights[i] = 0.0;
} else {
weights[i] = 1.0;
}
}
}else{
//this is tricky situation; try signalling outliers assuming Normal dist...
double s = STDEV(DIAM);
median_D = MEDIAN(DIAM);
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
if( abs( DIAM[i] - median_D ) > 2.0 * s ) {
weights[i] = 0.0;
} else {
weights[i] = 1.0;
}
}
}
}
//PERFORM 2ND ORDER POLYNOMIAL FIT
double a, b, c;
std::vector<double> temp_mad;
double w1 = 0.0;
double wx = 0.0, wx2 = 0.0, wx3 = 0.0, wx4 = 0.0;
double wy = 0.0, wyx = 0.0, wyx2 = 0.0;
double tmpx = 0.0, tmpy = 0.0;
double den;
double x, y, w;
if(DIAM.size()-SUM(weights)>2){
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
x = ARC[i];
y = DIAM[i];
w = weights[i];
w1 += w;
tmpx = w * x;
wx += tmpx;
tmpx *= x;
wx2 += tmpx;
tmpx *= x;
wx3 += tmpx;
tmpx *= x;
wx4 += tmpx;
tmpy = w * y;
wy += tmpy;
tmpy *= x;
wyx += tmpy;
tmpy *= x;
wyx2 += tmpy;
}
den = wx2 * wx2 * wx2 - 2.0 * wx3 * wx2 * wx + wx4 * wx * wx + wx3 * wx3 * w1 - wx4 * wx2 * w1;
if( den == 0.0 ) {
a = 0.0;
b = 0.0;
c = 0.0;
} else {
a = (wx * wx * wyx2 - wx2 * w1 * wyx2 - wx2 * wx * wyx + wx3 * w1 * wyx + wx2 * wx2 * wy - wx3 * wx * wy) / den;
b = (-wx2 * wx * wyx2 + wx3 * w1 * wyx2 + wx2 * wx2 * wyx - wx4 * w1 * wyx - wx3 * wx2 * wy + wx4 * wx * wy) / den;
c = (wx2 * wx2 * wyx2 - wx3 * wx * wyx2 - wx3 * wx2 * wyx + wx4 * wx * wyx + wx3 * wx3 * wy - wx4 * wx2 * wy) / den;
}
// FILL IN DATA
double d;
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
x = ARC[i];
d = a * x * x + b * x + c;
if( d > 1.0e-6 ) {
baseline[i] = d;
} else {
baseline[i] = 1.0e-6;
}
}
}else{
median_D = MEDIAN(DIAM);
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
baseline[i] = median_D;
}
}
// Do not allow the baseline to go bellow the minimal diameter, or above maximal diameter - truncate
MIN_ = MINIMUM(DIAM);
MAX_ = MAXIMUM(DIAM);
for(unsigned i = 0; i < Npoints; i = i + 1 ) {
if (baseline[i]<MIN_){
baseline[i] = MIN_;
}
if (baseline[i]>MAX_){
baseline[i] = MAX_;
}
}
return;
}
