// (c) 2003. Dinko Korunic 'kreator'
//
// This program is free software; you can redistribute it and/or modify it
// under the terms of the GNU General Public License as published by the
// Free Software Foundation; either version 2 of the License, or (at your
// option) any later version.
//
// Treca laboratorijska vjezba iz APR
//  Implementacija Simplex metode (po Nelder i Mead)

#include <stl.h>
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>

// makroi za skipanje razlicitih delimitera..
#define SkipDelimiterNL(ifs) SkipChar(ifs, " \n")
#define SkipDelimiters(ifs) SkipChar(ifs, " \t")

typedef vector<double> double_vector;
typedef vector<double_vector> double_matrix;

// osnovna klasa
class Matrica
{
  public:
    int redaka, stupaca; // podaci o podacima :-)
    double_matrix podaci; // 2D STL vektor sa elementima
    Matrica(); // osnovni konstruktor
    Matrica(const int, const int); // konstruktor sa dimenzijama
    Matrica(const int, const int, const double); // konstruktor sa dimenzijama
    Matrica(const Matrica &); // konstruktor sa kopiranjem
    Matrica& operator= (const Matrica &); // nadogr. pridruzivanje
    Matrica operator+ (const Matrica &); // nadogr. zbrajanje
    Matrica operator- (const Matrica &); // nadogr. oduzimanje
    Matrica operator* (const Matrica &); // nadogr. mnozenje
    Matrica operator* (double); // nadogr. mnozenje skalarom
    Matrica& operator+= (const Matrica &); // nadogr. +=
    Matrica& operator-= (const Matrica &); // nadogr. -=
    friend ostream& operator<< (ostream &, Matrica &); // nadogr. <<
    friend istream& operator>> (istream &, Matrica &); // nadogr. >>
};

typedef vector<Matrica> Matrica_vector;
typedef double(*f_handler_list_t)(const Matrica &);

// prazni konstruktor - treba nam samo za retke/stupce
Matrica::Matrica()
{
  redaka = stupaca = 0;
}

// konstruktor za matricu iz matrice
//      ulaz: referenca na izvornu matricu
//      izlaz: nista
Matrica::Matrica(const Matrica &izvor)
{
  *this = izvor;
}

// konstruktor za stvaranje nove matrice proizvoljnih dimenzija
//      ulaz: retci, stupci
//      izlaz: nista
Matrica::Matrica(const int _redaka, const int _stupaca) :
  redaka(_redaka), stupaca(_stupaca)
{
  double_matrix tmp(redaka, stupaca);
  podaci = tmp;
}

// konstruktor za stvaranje nove matrice proizvoljnih dimenzija
//      ulaz: retci, stupci, konstanta na koju ce biti svi elementi
//            postavljeni
//      izlaz: nista
Matrica::Matrica(const int _redaka, const int _stupaca, const double x) :
  redaka(_redaka), stupaca(_stupaca)
{
  double_matrix tmp(redaka, stupaca);
  podaci = tmp;
  int i, j;
  for (i = 0; i < redaka; ++i)
    for (j = 0; j < stupaca; ++j)
      podaci[i][j] = x;
} // Matrica::Matrica(int, int)

// operator pridruzivanja
//      ulaz: referenca na matricu
//      izlaz: referenca na vlastiti objekt
Matrica& Matrica::operator= (const Matrica &izvor)
{
  stupaca = izvor.stupaca;
  redaka = izvor.redaka; // izjednacimo stupce/retke
  podaci = izvor.podaci; // i onda same podatke (nadogr. =)

  return *this;
}

// operator zbrajanja
//      ulaz: referenca na matricu
//      izlaz: matrica (novi objekt)
Matrica Matrica::operator+ (const Matrica &izvor)
{
  if ((izvor.stupaca != stupaca)
      || (izvor.redaka != redaka))
    throw "operator+: matrice nemaju iste dimenzije";

  Matrica rezultat(*this); // koristimo nadogradjeno instanciranje
 
  int i, j;
  
  for (i = 0; i < redaka; ++i)
    for (j = 0; j < stupaca; ++j)
      rezultat.podaci[i][j] += izvor.podaci[i][j]; // i za sve elem.
  
  return rezultat;
}

// operator oduzimanja
//      ulaz: referenca na matricu
//      izlaz: matrica (novi objekt)
Matrica Matrica::operator- (const Matrica &izvor)
{
  if ((izvor.stupaca != stupaca)
      || (izvor.redaka != redaka))
    throw "operator+: matrice nemaju iste dimenzije";

  Matrica rezultat(*this);
 
  int i, j;
  
  for (i = 0; i < redaka; ++i)
    for (j = 0; j < stupaca; ++j)
      rezultat.podaci[i][j] -= izvor.podaci[i][j]; // za sve..
  
  return rezultat;
}

// operator mnozenja skalarom
//      ulaz: skalar
//      izlaz: matrica (novi objekt)
Matrica Matrica::operator* (const double izvor)
{
  Matrica rezultat(*this);
 
  int i, j;
  
  for (i = 0; i < redaka; ++i)
    for (j = 0; j < stupaca; ++j)
      rezultat.podaci[i][j] *= izvor; // za sve elemente
  
  return rezultat;
}

// operator mnozenja matricom
//      ulaz: referenca na matricu
//      izlaz: matrica (novi objekt)
Matrica Matrica::operator* (const Matrica &izvor)
{
  if (stupaca != izvor.redaka)
    throw "operator*: matrice nisu ulancane";

  Matrica rezultat(redaka, izvor.stupaca);

  int i, j, k;
  
  for (i = 0; i < redaka; ++i)
    for (j = 0; j < izvor.stupaca; ++j)
      for (k = 0; k < stupaca; ++k)
        rezultat.podaci[i][j] += podaci[i][k] * izvor.podaci[k][j];
        // prema formuli A[i][j] = Sum a[i][j]b[k][j], k=1..n

  return rezultat;
}

// operator zbrajanja i izjednacenja
//      ulaz: referenca na matricu
//      izlaz: referenca na matricu
Matrica& Matrica::operator+= (const Matrica &izvor)
{
  return (*this = *this + izvor); // najjednostavnije
}

// operator oduzimanja i izjednacenja
//      ulaz: referenca na matricu
//      izlaz: referenca na matricu
Matrica& Matrica::operator-= (const Matrica &izvor)
{
  return (*this = *this - izvor); // najjednostavnije
}

// preskacemo zeljene znakove, ma koliko ih god bilo
//      ulaz: input stream, znak za ignoriranje
//      izlaz: nista
void SkipChar(istream &ifs, char *igme)
{
  for (; index(igme, ifs.peek()) != NULL; ifs.ignore());
}

// operator >>
//      ulaz: input stream, referenca na matricu
//      izlaz: referenca na input stream
istream& operator>> (istream &ifs, Matrica &MOb)
{
  while (ifs.peek() != EOF) // dok mozes citaj
  {
    double tmp;

    MOb.stupaca = 0; // najjednostavnije resetirati svaki put
    double_vector tmpRedak; // privremeni redak

    while (ifs && ifs.peek() != '\n')
    {
      ifs >> tmp; // ucitaj jedan znak
      tmpRedak.push_back(tmp); // i jedan po jedan u vektor
      MOb.stupaca++;
      SkipDelimiters(ifs); // preskoci " " ili "\t"
    }
    MOb.podaci.push_back(tmpRedak); // redak u kompletnu tablicu
    SkipDelimiterNL(ifs); // preskoci "\n" i sl.
    MOb.redaka++;
  }

  return ifs;
}

// operator <<
//      ulaz: output stream, referenca na matricu
//      izlaz: referenca na output stream
ostream& operator<< (ostream &ofs, Matrica &MOb)
{
  double_matrix::iterator it;

  for (it = MOb.podaci.begin(); it != MOb.podaci.end(); ++it)
  {
    ostream_iterator<double> out(ofs, "\t");
    copy(it->begin(), it->end(), out); // iteriraj sve elemente
    ofs << endl;
  }

  return ofs;
}

// Ulazni parametri
//  x0 = pocetna tocka pretrazivanja
//  t = razmak izmedju tocaka
//  n = dimenzija simplexa
//  f je funkcija koju optimiramo double f(const Matrica)
//  a = alpha
//  b = beta
//  g = gamma
//  e = epsilon
Matrica Simplex(const Matrica &x0, double &t, int n,
    double(*f)(const Matrica &), double &a, double &b, double &g, double &e)
{
  // izracunamo koeficijente a1 i a2
  double a1, a2;
  a1 = t * (sqrt((double)n + 1.) + (double)n - 1.) / ((double)n * sqrt(2.));
  a2 = t * (sqrt((double)n + 1.) - 1.) / ((double)n * sqrt(2.));
  
  // stvori vektor Matrica svih d-ova
  Matrica d_prototip(n, 1, a2); // prototip d koji ima sve a2 elemente
  Matrica_vector d(n, d_prototip); // generiraj vektor pun prototipova
  int i;
  for (i = 0; i < n; ++i)
    d[i].podaci[i][0] = a1; // popuni sve a1 elemente na prava mjesta
 
  // stvori vektor Matrica svih x-ova
  vector <Matrica> x(n + 1, x0);
  for (i = 1; i <= n; ++i)
    x[i] += d[i - 1]; // i izracunaj xj tocaka

  // par potrebnih varijabli
  Matrica xr, xe, xk, xc, prazni(n, 1, 0.);
  int h, l;
  double Fxh, Fxl, tmp;

  do
  {
    // pronadji xh i xl sa najvecom i najmanjom vrijednosti funkcije cilja
    h = l = 0;
    Fxh = f(x[h]);
    Fxl = f(x[l]);
    for (i = 1; i <= n; ++i)
    {
      if ((tmp = f(x[i])) > Fxh) // max
      {
        Fxh = tmp;
        h = i;
      }
      if (tmp < Fxl) // min
      {
        Fxl = tmp;
        l = i;
      }
    }

    // izracunamo centroid
    xc = prazni;
    for (i = 0; i <= n; ++i)
    {
      if (i != h)
        xc += x[i];
    }
    xc = xc * (1./(double)n);

    xr = xc * (1. + a) - x[h] * a; // refleksija
    if (f(xr) < Fxl)
    {
      xe = xc * (1 - g) + xr * g; // ekspanzija
      if (f(xe) < Fxl)
        x[h] = xe;
      else
        x[h] = xr;
    }
    else
    {
      int simplex_flag = 1;
      for (i = 0; (i <= n) && simplex_flag; ++i)
      {
        if ((i != h) && (f(xr) <= f(x[i])))
          simplex_flag = 0;
      }
      if (simplex_flag)
      {
        if (f(xr) < f(x[h]))
          x[h] = xr;
        xk = xc * (1. - b) + x[h] * b; // kontrakcija
        if (f(xk) < f(x[h]))
          x[h] = xk;
        else
        {
          // pomakni sve tocka ka x[l] na pola razmaka
          for (i = 0; i <= n; ++i)
            if (i != l)
              x[i] = (x[l] + x[i]) * 0.5;
        }
      }
      else
        x[h] = xr;
    }

    // zavrsni uvjet
    tmp = 0.;
    for (i = 0; i <= n; ++i)
      tmp += pow(f(x[i]) - f(xc), 2.);
    tmp = sqrt (tmp / (double)n);
  } while (tmp > e);

  return xc;
}

// funkcija: f1(x,y) = 10*(x2-y)^2+(1-x)^2
double f1(const Matrica &x)
{
  double x1 = x.podaci[0][0], y1 = x.podaci[1][0];
  return 10. * (x1 * x1 - y1) * (x1 * x1 - y1)  + (1 - x1) * (1 - x1);
}

// funkcija: f2(x,y) = (x-4)^2 + 4*(y-2)^2
double f2(const Matrica &x)
{
  double x1 = x.podaci[0][0], y1 = x.podaci[1][0];
  return (x1 - 4.) * (x1 - 4) + 4 * (y1 - 2) * (y1 - 2);
}

Matrica f3_const;
// f3(x1,x2,x3,x4,x5) = (x1-p1)^2 + (x2-p2)^2 + ... + (x5-p5)^2
double f3(const Matrica &x)
{
  int i;
  double sum = 0, tmp;
  
  for (i = 0; i < 5; ++i)
  {
    tmp = x.podaci[i][0] - f3_const.podaci[i][0];
    sum += tmp * tmp;
  }

  return sum;
}

// f4(x,y) = |(x-y)*(x+y)| + (x^2+y^2)^0.5
double f4(const Matrica &x)
{
  double x1 = x.podaci[0][0], y1 = x.podaci[1][0];
  return fabs((x1 - y1) * (x1 + y1)) + pow(x1 * x1 + y1 * y1, 0.5);
}

// glavna funkcija
int main()
{
  Matrica rezultat, pocetna_tocka;
  double a = 1., b = 0.5, g = 2., e = 10e-9, t = 1.;
  int odgovor = 0;
  f_handler_list_t f_list[] = {f1, f2, f3, f4};

  while ((odgovor < 1) || (odgovor > 4))
  {
    cout << "Koju zelis funkciju optimirati? ";
    cin >> odgovor;
  }
 
  try
  {
    ifstream ParamFile("apr3-param");
    if (ParamFile)
    {
      ParamFile >> a >> b >> g >> e >> pocetna_tocka;
      ParamFile.close();
    }
    
    if (odgovor == 3)
    {
      ifstream CoefFile("apr3-coef");
      if (!CoefFile)
        throw "Ne mogu otvoriti datoteku sa koeficijentima";
      else
        CoefFile >> f3_const;
      CoefFile.close();
      
      if (f3_const.redaka != 5)
        throw "Krive dimenzije matrice koeficijenata";
  
      if ((pocetna_tocka.redaka != 5) || (pocetna_tocka.stupaca != 1))
      {
        Matrica pocetna_tocka2(5, 1, 0.);
        pocetna_tocka = pocetna_tocka2;
      }
      rezultat = Simplex(pocetna_tocka, t, 5, f_list[odgovor - 1], a, b,
          g, e);
    }
    else
    {
      if ((pocetna_tocka.redaka != 2) || (pocetna_tocka.stupaca != 1))
      {
        Matrica pocetna_tocka2(2, 1, 0.);
        pocetna_tocka = pocetna_tocka2;
      }
      rezultat = Simplex(pocetna_tocka, t, 2, f_list[odgovor - 1], a, b,
          g, e);
    }
    cout << "Rezultat je " << endl << rezultat << endl;
  }

  catch (const char *msg)
  {
    cout << "Greska: " << msg << endl;
    return EXIT_FAILURE;
  }
  
  return EXIT_SUCCESS;
} // main