#include<stdio.h>
#include<math.h>
#include<stdlib.h>
#include <sys/time.h>
#include <unistd.h>

typedef double* p_double;

//constants and variables of fitness function:

#define Dim 2
#define xmin -200.0
#define xmax 200.0
#define r0 1.0
#define y0min 10.0
#define y0max 50.0
#define precision 0.001

double y_0, x_0[Dim];

//constants and variables of SADE:

#define PoolSize 20
#define SelectedSize 10

#define radioactivity 0.05
#define mutation_rate 0.5
#define mutagen 1.0

#define cross_rate 0.1

int ActualSize, mutants;
double Force[PoolSize], reduced_radioactivity, top;
p_double *CH;

void initialize_randoms ( void )
{
    int rseed;

    struct timeval tv;
    struct timezone tz;

    gettimeofday ( &tv, &tz );

    rseed = tv.tv_usec + ( tv.tv_sec%4194304 )*1000;

    srand ( rseed );
}

double random_double ( double olow, double ohigh )
{
    return (( double )rand()/RAND_MAX )*( ohigh-olow )+olow ;
}

int random_int ( int olow, int ohigh )
{
    return rand()%( ohigh-olow )+olow;
}

double fitness ( double *ox )
{
    double y=0.0;
    int i;
    
    for ( i=0; i<Dim; i++ )
    {
        y += ( ox[i]-x_0[i] )*( ox[i]-x_0[i] );
    }
    y = -sqrt( y )/r0;
    
    return y_0*( atan( y )+M_PI_2 );
}

void create_fitness_function ( void )
{
    int i;

    y_0 = random_double ( y0min, y0max );
    for ( i=0; i<Dim; i++ )
    {
        x_0[i] = random_double ( xmin, xmax );
    }
    top = fitness ( x_0 );
    printf ( "%f\n",top );
    
}

double* new_point ( void )
{
    int i;
    double *x;

    x = new double [Dim];
    for ( i=0; i<Dim; i++ )
    {
        x[i] = random_double ( xmin, xmax );
    }
    return x;
}

void compute_radioactivity ( void )
{
    double p, rn;

    p = radioactivity*( double )SelectedSize;
    rn = ceil ( p );
    reduced_radioactivity = p/rn;
    mutants = ( int )rn;
}

double EVALUATE_GENERATION ( void )
{
    double btg=0.0;
    int i;
    static int start=0;

    for ( i=start; i<ActualSize; i++ )
    {
        Force[i] = fitness ( CH[i] );
        if ( Force[i]>btg ) btg = Force[i];
    }

    if ( start==0 ) start = SelectedSize;

    return btg;
}	

void SELECT ( void )
{
    double *h;
    int i1, i2, dead, last;

    while ( ActualSize > SelectedSize )
    {
        i1 = random_int ( 0,ActualSize );
        i2 = random_int ( 1,ActualSize );
        if ( i1==i2 ) i2--;
        if ( Force[i1] >= Force[i2] ) dead = i2;
        else dead = i1;
        last = ActualSize-1;
        h = CH[last];
        CH[last] = CH[dead];
        CH[dead] = h;
        Force[dead] = Force[last];
        ActualSize--;
    }
}	

void MUTATE ( void )
{
    double p, *x;
    int i, j, index;

    for ( i=0; i<mutants; i++ )
    {
        p = random_double ( 0,1 );
        if ( p<=reduced_radioactivity )
        {
            index = random_int ( 0,SelectedSize );
            x = new_point();
            for ( j=0;  j<Dim; j++ )
            {
                CH[ActualSize][j] = CH[index][j]+mutation_rate*( x[j]-CH[index][j] );
            }
            delete [] x;
            ActualSize++;
        }
    }
}

void BOUNDARY_MUTATE ( void )
{
    double p,dCH ;
    int i,j,index ;
    for ( i=0; i<mutants; i++ )
    {
        p = random_double( 0,1 );
        if ( p <= reduced_radioactivity )
        {
            index = random_int( 0, SelectedSize );
            for ( j=0; j<Dim; j++ )
            {
                dCH = random_double( -mutagen,mutagen );
                CH[ActualSize][j] = CH[index][j]+dCH;
            }
            ActualSize++;
        }
    }
}

void CROSS ( void )
{
    int i1,i2,i3,j ;
    while ( ActualSize < PoolSize )
    {
        i1 = random_int( 0,SelectedSize );
        i2 = random_int( 1,SelectedSize );
        if ( i1==i2 ) i2--;
        i3 = random_int( 0,SelectedSize );
        for ( j=0 ; j<Dim ; j++ )
        {
            CH[ActualSize][j] = CH[i3][j]+cross_rate*( CH[i2][j]-CH[i1][j] ) ;
        }
        ActualSize++;
    }
}

double FIRST_GENERATION ( void)
{
    int i;
    double bsf;

    CH = new p_double [PoolSize];
    for ( i=0; i<PoolSize; i++ )
    {
        CH[i] = new_point ();
    }
    ActualSize = PoolSize;
    bsf = EVALUATE_GENERATION ();
    SELECT ();
    return bsf;
}	

int to_continue ( double bsf )
{
    if ( top-bsf > precision ) return 1;
    return 0;
}

void destroy_all ( void )
{
    int i;

    for ( i=0; i<PoolSize; i++ ) delete [] CH[i];
    delete [] CH;
}

void print_self ( double bsf, int end=0 )
{
    static int i=0, g=0;

    i++;
    if ( i==50 )
    {
        i=0;
        g++;
        printf ( "G=%d bsf=%f top=%f\n", 50*g+i, bsf, top );
    }
    if ( end ) printf ( "G=%d bsf=%f top=%f\n", 50*g+i, bsf, top );
}

void main ( void )
{
    double bsf;

    initialize_randoms ();
    create_fitness_function ();
    compute_radioactivity ();

    bsf = FIRST_GENERATION ();

    while ( to_continue ( bsf ) )
    {
        MUTATE ();
        BOUNDARY_MUTATE ();
        CROSS ();
        bsf = EVALUATE_GENERATION ();
        SELECT();
        print_self ( bsf );
    }
    print_self ( bsf, 1 );

    destroy_all ();
}