#include <stdio.h>
#include <stdint.h>
#include <math.h>
// Sean O'Connor 23 January 2014.  Hoang Hoa Tham, Ba Dinh, Hanoi.
// Return the position of the highest set bit.
uint32_t topBit(uint32_t x){         
        return ilogbf(x);
}

// Fill back all bits from the highest set bit.
uint32_t fillBack(uint32_t x){         
        x |= x>>1;
        x |= x>>2;
        x |= x>>4;
        x |= x>>8;
        x |= x>>16;
        return x;
}

// Xorshift128 RNG + inject randomization to allow seeding.
uint32_t rnd32(uint32_t inject){   
  static uint32_t x = 123456789;
  static uint32_t y = 362436069;
  static uint32_t z = 521288629;
  static uint32_t w = 88675123;
  x+=inject;
  uint32_t t = x ^ (x << 11);
  x = y; y = z; z = w;
  w ^= (w >> 19) ^ t ^ (t >> 8);
  return w;
}

// A random number from the inclusive interval.
uint32_t rndInterval(uint32_t start,uint32_t end){
        uint32_t r=end-start;
        uint32_t m=fillBack(r);
        uint32_t x;
        while((x=rnd32(0)&m)>r);
        return x+start;
}

// Mutates the parent value with random number whose value
// is ranged on one of a number of equally likely scales.
// the parent value should be between -1 and 1 inclusive.
float midas(float pv,uint32_t baseExp,uint32_t topExp){
        float m=0,cv;
        uint32_t x=rnd32(0)&0x807fffff;                        // rnd mutation and mask out the exponent of the future 32 bit floating point number
        uint32_t y=rndInterval(baseExp,topExp); // decide on a characteristic scale for the mutation
        if(y>127) y=127;                                                // limit the mutation exponent to 127 (fp values -2 to 2)
        *(uint32_t*)&m=x+(y<<23);                                // construct the fp number from its sign,mantissa and exponent
        cv=m+pv;
        if(cv<-1 || cv>1) cv=pv;                                // if the mutated value is out of bounds then there is no mutation
        return cv;
}

// Canterlot (Scale Free) Evolution Strategy.
// Provides scale free mutations for doing numerical optimizations etc.  The values in the parent
// vector should be between -1 and 1 inclusive.
// The precision value is between 0 and 127. 127 being maximum precison (slower but more perfect).
// The over the top (ott) parameter provides the ability to produce really high magnitude mutations if needed.
void canterlot(float *parent,float *child,uint32_t n,uint32_t precision,uint32_t ott){
        uint32_t maxshift,shift,idx,rExp,baseExp,topExp;
        baseExp=127-precision;                                        // the lowest exponent value to be used in the mutation f.p. number.
        topExp=rndInterval(baseExp,127+ott);        // virtual highest exponent value to be used in f.p. mutation vector (will be clipped to 127)
        maxshift=topBit(n);                                                // number of divisions of population size (1,2,3 to 4,5 to 8,9 to 16,...,25% to 50%,50% to 100%) possible.
        shift=rndInterval(0,maxshift);                        // select a division uniformly at random (a single mutation is as likely to occur as say 9 to 16 mutations).
        rExp=rnd32(0) | 0x80000000;                                // msb set+31 random bits
        if(shift<2){
                rExp >>=shift;                                                                                // shift rExp right 0 or 1 places.
                for(uint32_t i=0;i<n;i++){         
                        if(rnd32(0)<=rExp){                                                                // 25 To 50 or 50 to 100% of the time.
                                child[i]=midas(parent[i],baseExp,topExp);        // child gets mutated value.
                        }else{
                                child[i]=parent[i];                                                        // child gets unmutated value.
                        }
                }
                idx=rndInterval(0,n-1);                                                                // shoe in at least 1 topExp scale mutation.
                child[idx]=midas(parent[idx],topExp,topExp);         
        }else{
                for(uint32_t i=0;i<n;i++){
                        child[i]=parent[i];                                                                // first copy across parent vector to child vector.
                }         
                idx=rndInterval(0,n-1);         
                child[idx]=midas(parent[idx],topExp,topExp);                // do at least 1 topExp scale mutation.
                if(shift!=2){                                                                         
                        rExp >>=34-shift;                                                                  // do a shift of rExp to see how many further mutations to make, (1,2 to 3, 4 to 7).
                        for(uint32_t i=0;i<rExp;i++){
                                do{
                                        idx=rndInterval(0,n-1);                                        // select one index at random.
                                }while(parent[idx]!=child[idx]);                        // if definitely already mutated then try a different index.
                                child[idx]=midas(parent[idx],baseExp,topExp);  // do the mutation.
                        }         
                }
        }
}

// replace with a good test function
float cost(float *vec,uint32_t n){
        float r=0;
        for(uint32_t i=0;i<n;i++){
                r+=vec[i]*vec[i];
    }
        return r;
}

int main(int argc, char **argv){
        float parentCost,childCost=0,parent[20],child[20];
        for(uint32_t j=0;j<20;j++) parent[j]=1;
        rnd32(442523);        // seed random number generator
        parentCost=cost(parent,20);
        for(uint32_t i=0;i<20000;i++){
                canterlot(parent,child,20,32,0);
                childCost=cost(child,20);
                if(childCost<parentCost){
                        parentCost=childCost;
                        for(uint32_t j=0;j<20;j++) parent[j]=child[j];
            }
        }
        for(uint32_t i=0;i<20;i++) printf("%f\n",parent[i]);
        return 0;
}