#include "simulated_annealing.h"

double sa_rand ()
{
  return ((double)rand())/((double) RAND_MAX);
}

sa_param *sa_param_alloc (unsigned int N)
{
  #ifdef SA_DEBUG
  if (N==0) {
    fprintf(stderr, "%s:%i: Nelze vytvorit mnozinu parametru s poctem parametru %i.\n", __FILE__, __LINE__, N);
    exit(1);
  }
  #endif
  unsigned int i;
  sa_param *p = malloc(sizeof(sa_param));
  p->n = N;
  p->x = malloc(sizeof(double*)*N);
  p->dmax = malloc(sizeof(double)*N);
  // Inicializace velikosti kroku.
  for (i = 0; i<N; i++) {
    p->dmax[i] = 1;
  }
  return p;
}

void sa_param_free (sa_param *p)
{
  free(p->x);
  free(p);
}

void sa_param_define (sa_param *p, unsigned int i, double *param)
{
  #ifdef SA_DEBUG
  if (p->n > i) {
  #endif
  p->x[i] = param;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze definovat parametr s indexem %i, ktery je vetsi nez mnozstvi parametru %i.\n", __FILE__, __LINE__, i, p->n);
    exit(1);
  }
  #endif
}

double sa_param_get (sa_param *p, unsigned int i)
{
  #ifdef SA_DEBUG
  if (p->n > i) {
  #endif
  return *(p->x[i]);
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze ziskat parametr s indexem %i, ktery je vetsi nez mnozstvi parametru %i.\n", __FILE__, __LINE__, i, p->n);
    exit(1);
  }
  #endif
}

void sa_param_set (sa_param *p, unsigned int i, double value)
{
  #ifdef SA_DEBUG
  if (p->n > i) {
  #endif
  p->x[i][0] = value;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze nastavit parametr s indexem %i, ktery je vetsi nez mnozstvi parametru %i.\n", __FILE__, __LINE__, i, p->n);
    exit(1);
  }
  #endif
}

double sa_param_dmax_get (sa_param *p, unsigned int i)
{
  #ifdef SA_DEBUG
  if (p->n > i) {
  #endif
  return p->dmax[i];
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze ziskat maximalni moznou zmenu pro parametr s indexem %i, ktery je vetsi nez mnozstvi parametru %i.\n", __FILE__, __LINE__, i, p->n);
    exit(1);
  }
  #endif
}

void sa_param_dmax_set (sa_param *p, unsigned int i, double d)
{
  #ifdef SA_DEBUG
  if (p->n > i) {
  #endif
  p->dmax[i] = d;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze nastavit maximalni moznou zmenu pro parametr s indexem %i, ktery je vetsi nez mnozstvi parametru %i.\n", __FILE__, __LINE__, i, p->n);
    exit(1);
  }
  #endif
}

double *sa_get_params (sa_param *p)
{
  double *x = malloc(sizeof(double)*p->n);
  unsigned int i;
  for (i = 0; i<p->n; i++) {
    x[i] = sa_param_get(p, i);
  }
  return x;
}

void sa_param_print (sa_param *p)
{
  unsigned int i;
  for (i = 0; i < p->n; i++) {
    printf("p[%i] = %f\n", i, sa_param_get(p, i));
  }
}

sa_vector * sa_vector_alloc (unsigned int n)
{
  #ifdef SA_DEBUG
  if (n==0) {
    fprintf(stderr, "%s:%i: Nelze vytvorit vektor s nulovym poctem prvku.\n", __FILE__, __LINE__);
    exit(1);
  }
  #endif
  sa_vector *vec = malloc(sizeof(sa_vector));
  vec->n = n;
  vec->x = malloc(sizeof(double)*n);
  return vec;
}

void sa_vector_free (sa_vector *vec)
{
  free(vec->x);
  free(vec);
}

void sa_vector_set (sa_vector *vec, unsigned int i, double value)
{
  #ifdef SA_DEBUG
  if (vec->n > i) {
  #endif
  vec->x[i] = value;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze nastavit parametr s indexem %i, ktery je vetsi nez velikost vektoru %i.\n", __FILE__, __LINE__, i, vec->n);
    exit(1);
  }
  #endif
}

double sa_vector_get (sa_vector *vec, unsigned int i)
{
  #ifdef SA_DEBUG
  if (vec->n > i) {
  #endif
  return vec->x[i];
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze vratit hodnotu s indexem %i, ktery je vetsi nez velikost vektoru %i.\n", __FILE__, __LINE__, i, vec->n);
    exit(1);
  }
  #endif
}

int sa_vector_from_param (sa_vector *vec, sa_param *p)
{
  unsigned int i;
  #ifdef SA_DEBUG
  if (vec->n == p->n) {
  #endif
  for (i = 0; i < vec->n; i++) {
    sa_vector_set(vec, i, sa_param_get(p, i));
  }
  return 1;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze naplnit mnozinu parametru hodnotami z vektoru, protoze maji ruzne velikosti (card|vec| = %i , card|p| = %i).\n", __FILE__, __LINE__, vec->n, p->n);
    exit(1);
  }
  #endif
  return 0;
}

int sa_param_from_vector (sa_param *p, sa_vector *vec)
{
  unsigned int i;
  #ifdef SA_DEBUG
  if (vec->n == p->n) {
  #endif
  for (i = 0; i < vec->n; i++) {
    sa_param_set(p, i, sa_vector_get(vec, i));
  }
  return 1;
  #ifdef SA_DEBUG
  } else {
    fprintf(stderr, "%s:%i: Nelze naplnit vektor hodnotami z mnoziny parametru, protoze maji ruzne velikosti (card|vec| = %i , card|p| = %i).\n", __FILE__, __LINE__, vec->n, p->n);
    exit(1);
  }
  #endif
  return 0;
}

double sa_mean (double *x, unsigned int n)
{
  unsigned int i;
  double EX;
  if (n == 0) {
    return 0;
  }
  EX = 0;
  for (i = 0; i<n; i++) {
    EX += x[i];
  }
  EX = EX/((double)n);
  return EX;
}

double sa_stdev(double* x, unsigned int n)
{
  unsigned int i;
  double VARX, EX;
  if (n == 0) {
    return 0;
  }
  EX = sa_mean(x, n);
  VARX = 0;
  for (i = 0; i<n; i++) {
    VARX += pow(x[i]-EX, 2);
  }
  VARX = sqrt(VARX/((double)n));
  return VARX;
}

double sa_metropolis (sa_param *params, unsigned int kmax, double T, double dxmax, double (*energy) (void *), void *data, sa_metropolis_info *info)
{
  // Alokace puvodniho vektoru.
  sa_vector *old_p = sa_vector_alloc(params->n);
  // Do puvodniho vektoru ulozim hodnoty soucasnych parametru.
  if (sa_vector_from_param(old_p, params)==0) {
    fprintf(stderr, "%s:%i: Nepodarilo se nastavit vektor z hodnot mnoziny parametru.\n", __FILE__, __LINE__);
    exit(1);
  }
  // Energie soucasneho systemu.
  double E = info->E;
  double Enew;
  unsigned int k;

  info->acceptedStates = 0;
  info->betterStates = 0;
  info->worseStates = 0;
  info->dE[0] = 0;

  for (k = 0; k < kmax; k++) { // Mozna zmena stavu probehne KMAX-krat.

    sa_opert(params, dxmax); // Uprava mnoziny parametru PARAMS (nahodne vygenerovani noveho stavu ze soucasneh).
    Enew = energy(data); // Energie po zmene parametru.

    // Metropolisovo kriterium.
    // Rozhodne pokud zmenene parametry zustanou nebo se vrati na predesle hodnoty OLD_P.
    if (sa_rand()<=sa_pr_min(E, Enew, T)) {
      /*
       * Ponecha se zmenene nastaveni parametru a zaroven se ulozi do vektoru
       * OLD_P.
       */
      if (E>Enew) {
        info->betterStates++;
      }
      if (Enew>E) {
        info->worseStates++;
      }

      info->dE[info->acceptedStates] = Enew-E;
      info->acceptedStates++;
      
      if (sa_vector_from_param(old_p, params)==0) {
        fprintf(stderr, "%s:%i: Nepodarilo se nastavit vektor z hodnot mnoziny parametru.\n", __FILE__, __LINE__);
        exit(1);
      }
      E = Enew; // Ulozeni energie noveho stavu.
    } else {
      /*
       * Parametry se nastavi na puvodni.
       */
      if (sa_param_from_vector(params, old_p)==0) {
        fprintf(stderr, "%s:%i: Nepodarilo se nastavit mnozinu parametru z hodnot vektoru.\n", __FILE__, __LINE__);
        exit(1);
      }

    }

  }

  // Uvolneni pomocnych promenych.
  sa_vector_free(old_p);
  
  // Ulozeni chyby pri soucasne konfiguraci parametru.
  info->E = E;

  return E;

}

void sa_opert (sa_param *params, double dxmax)
{
  double new_param;
  unsigned int i;
  // Vsechny parametry z X se ulozi do XNEW s malou zmenou < -DXMAX , + DXMAX >
  for (i = 0; i<params->n; i++) {
    new_param = sa_param_get(params, i);
    new_param += 2*(sa_rand()-0.5)*dxmax*sa_param_dmax_get(params, i);
    sa_param_set(params, i, new_param);
  }
}

double sa_pr_min (double E, double Enew, double T)
{
  if (Enew<=E) {
    return 1;
  } else {
    return exp(-(Enew-E)/T);
  }
}

void sa_algorithm (sa_param *params, sa_algo *algo)
{
  unsigned int iter = 1;
  double Tmax = algo->Tmax; // pocatecni teplota
  double Tmin = algo->Tmin; // minimalni teplota
  double alfa = algo->alfa; // rychlost klesani teploty
  double beta = algo->beta; // rychlost klesani rozsahu
  double (*energy)(void*); // chybova funkce
  energy = algo->energy;
  void *data = algo->data;
  double dmax = algo->dmax; // maximalni rozsah zmen parametru pri generovani noveho savu
  unsigned int kmax = algo->kmax; // maximalni pocet iteraci pri konstantni teplote
  double T = Tmax; // Aktualni teplota
  double E, lastE; // Soucasna chyba a chyba v minule iteraci
  double d = dmax; // maximalni rozsah pro zmenu parametru
  double varE, meanE; // prumer chyby v jedne iteraci
  char buffer[1000];
  FILE *logfile;
  size_t buffer_size;
  time_t s, e; // pro stopovani casu
  double dt;

  sa_metropolis_info info;
  double ME[kmax];
  info.dE = ME;
  lastE = info.E = energy(data);
  printf ("Simulated Annealing\nTmax = %f\nTmin = %f\nalfa = %f\nk = %i\n0. initial E = %f\n", Tmax, Tmin, alfa, kmax, lastE);
  
  logfile = fopen(algo->logfile, "a+");
  sprintf(buffer, "Simulated Annealing\nTmax = %f\nTmin = %f\nalfa = %f\nk = %i\n0. initial E = %f\n", Tmax, Tmin, alfa, kmax, lastE);
  buffer_size = strlen(buffer);
  fwrite((void*) buffer, 1, buffer_size, logfile);
  fclose(logfile);

  while (T>Tmin) {
    time(&s);
    E = sa_metropolis(params, kmax, T, d, energy, data, &info);
    time(&e);
    // Zakladni statistika prubehu zmeny stavu.
    meanE = sa_mean(info.dE, info.acceptedStates);
    varE = sa_stdev(info.dE, info.acceptedStates);
    
    sprintf(buffer ,"%i. T = %f , d = %f , E = %f    accepted states %i ( dE = %f mean(dE) = %f stdev(dE) = %f ) , better %i , worse %i , time = %.0fs\n", iter, T, d, E, info.acceptedStates, E-lastE, meanE, varE, info.betterStates, info.worseStates, difftime(e, s));
    printf("%s", buffer); // Vytiskuti prubehu optimalizace
    lastE = E;

    // Ulozeni logu
    if (algo->logfile!=NULL) {
      buffer_size = strlen(buffer);
      logfile = fopen(algo->logfile, "a+");
      fwrite((void*) buffer, 1, buffer_size, logfile);
      fclose(logfile);
    }

    iter++;
    T = T*(1-alfa); // Snizeni teploty
    d = d*(1-beta); // Snizeni rozsahu pri generovani novych stavu
  }

}


int sa_opt_save(const char *filename, sa_algo *opt)
{
  char save_string[1000];
  char buffer[100];
  // ulozeni koeficientu snizovani teploty
  sprintf(save_string, "alfa, beta, dmax\n%f\n", opt->alfa);
  // ulozeni koeficientu snizovani pohybu parameteru
  sprintf(buffer, "%f\n", opt->beta);
  strcat(save_string, buffer);
  // ulozeni mozne zmeny parametru
  sprintf(buffer, "%f\n", opt->dmax);
  strcat(save_string, buffer);
  FILE *pf = fopen(filename, "w");
  return fputs( save_string, pf);
}


sa_algo *sa_opt_load(const char *filename)
{
  FILE * pFile;
  long lSize;
  char * buffer;
  size_t result;

  pFile = fopen(filename , "rb");
  if (pFile==NULL) {fputs ("File error",stderr); exit (1);}

  // obtain file size:
  fseek (pFile , 0 , SEEK_END);
  lSize = ftell (pFile);
  rewind (pFile);

  // allocate memory to contain the whole file:
  buffer = (char*) malloc (sizeof(char)*lSize);
  if (buffer == NULL) {fputs ("Memory error",stderr); exit (2);}

  // copy the file into the buffer:
  result = fread (buffer,1,lSize,pFile);
  if (result != lSize) {fputs ("Reading error",stderr); exit (3);}

  sa_algo *opt = malloc(sizeof(sa_algo));

  char *pch;
  pch = strtok(buffer, "\n");

  // ulozim alfa
  pch = strtok(NULL, "\n");
  scanf("%f", &(opt->alfa));
  // ulozim beta
  pch = strtok(NULL, "\n");
  scanf("%f", &(opt->alfa));
  // ulozim dmax
  pch = strtok(NULL, "\n");
  scanf("%f", &(opt->alfa));

  // vratim nastaveni algoritmu
  return opt;
}
