#include <stdio.h>
#include "ode.h"

/******************************************
Dette er implementasjonen for class odeSolver.

Se kommentarene for hver enkelt metode, og 
også dokumentasjonen ode.txt.

---

Copyright 2000, Simen Kvaal, men for pokker,
gjør hva du vil med koden!
*******************************************/

/*
  Setter alle vektorerpointere til NULL.
  Enda godt denne er private!;)
  (Kalles _kun_ av constructor for å forsikre
  at ikke-allokerte arrayer er NULL.)
*/
void odeSolver::nullAll(void) {
  k1 = NULL;
  k2 = NULL;
  k3 = NULL;
  k4 = NULL;
  dydt = NULL;
  y_temp = NULL;
  y = NULL;
  next_y=NULL;
  
}

/*
  Sletter allokert minne for vektorer. Privat
  funksjon som bruker ikke skal rote seg borti.
  (Dette er heller ikke nødvendig og kunne eventuelt
  ført til problemer med minnehåndtering.)
 */
void odeSolver::killAll(void) {
  //Delete alle arrays som er allokert.
  //Dette kan skape problemer dersom kompileren
  //_ikke_ nullstiller alle variable by default.
  //(Undersøkelser tyder på at dette er tilfelle
  //dersom objektet ikke er dynamisk allokert.)

  if (k1) delete[] k1;
  if (k2) delete[] k2;
  if (k3) delete[] k3;
  if (k4) delete[] k4;
  if (y_temp) delete[] y_temp;
  if (y) delete[] y;
  if (next_y) delete[] next_y;
  if (dydt) delete[] dydt;

  //Nullstill arrays.
  nullAll();
}

/*
  Tester om odeSolver er riktig initialisert, dvs.
  om k1...k4 og disse samt derivertfunksjonen eksisterer/er allokert.
  Funksjonen kan brukes av bruker, men blir også brukt
  av f.eks. calculate().
*/
int odeSolver::isInitialized(void) {
  if ((k1)&&(k2)&&(k3)&&(k4)&&(next_y)&&(y_temp)&&(y)&&(dydt)&&(deriv))
    return 1;
  else
    return 0;
}

/*
  Dummy constructor. Allokerer ikke minne og
  velger heller ikke en derivertfunksjon.
  Bruker må selv sette disse med setNumberOf-
  Equations() og setDerivativeFunction().
  h og t blir satt til hhv. 0.1 og 0.0.
  Se også den andre constructoren.
*/
odeSolver::odeSolver() {
  nullAll();
  killAll(); //Setter alle  pointere til null.
  setDerivativeFunction(NULL);
  setAlgorithm(ODE_RK4);
  setTime(0.0);
  setStepSize(0.1);
}

/*
  Constructor. Setter antall likninger til n og allokerer
  minne. (Se setNumberOfEquations()), setter derivertfunksjon
  til func og algoritme til RK_4 ved deafult.
  h og t settes til hhv 0.1 og 0.0; se setTime() og
  setStepSize().
*/

odeSolver::odeSolver(unsigned n, derivPtr func) {
  nullAll();
  killAll();
  setNumberOfEquations(n);
  setDerivativeFunction(func);
  setAlgorithm(ODE_RK4);
  setTime(0.0);
  setStepSize(0.1);
}
/*
  Destructor. Rydder opp i allokert minne.
*/
odeSolver::~odeSolver() {
  killAll(); //Rydd opp i minnet.
}

/*
  Lar bruker velge algoritme for løsning av difflikning.
  Gyldige verdier er ODE_EULER==1 og ODE_RK4==4.
  Andre verdier gir feilmelding.
 */
int odeSolver::setAlgorithm(unsigned algo) {
  if ((algo==ODE_RK4)||(algo==ODE_EULER))
    algorithm = algo;
  else {
    return 0;
  }
  return 1;

}
int odeSolver::setTime(double theTime) {
  t = theTime;
  return 1;
}
double odeSolver::getTime(void) {
  return t;
}
double odeSolver::getLastCalcTime(void) {
  return (getTime()-getStepSize());
}

int odeSolver::setStepSize(double stepSize) {
  if (stepSize > 0.0)
    h = stepSize;
  else {
    return 0;
  }
  return 1;
}
double odeSolver::getStepSize(void) {
  return h;
}

/*
  Returnerer valgt algoritme. Se også setAltogithm().
*/
unsigned odeSolver::getAlgorithm(void) {
  return algorithm; //Denne vil alltid være gyldig. Er initialisert i constr.
}

/*
  Setter odeSolverens brukerdefinerte derivert-funksjon.
  Denne skal ta en double og en double-array som parameter:
    func(double t, double *y),
  Der t er tiden og y er funksjonsvektoren.

  Funksjonen må ikke skrive ut over det antall likninger
  som er ment å løses, og dette er brukers ansvar.
 */
int odeSolver::setDerivativeFunction(derivPtr func) {
  deriv = func;
  return 1;
}
/*
  Hent ut antall likninger som på dette tidspunkt
  er allokert.
 */
unsigned odeSolver::getNumberOfEquations(void) {
  if (!(isInitialized()))
    return 0;
  else
    return n_eq;
}

/*
  Setter antall samtidige likninger.
  Rydder først opp etter eventuelle tidligere
  likninger ved å delete[] arrayene.
  Allokerer minne på nytt deretter.
  Se også getNumberOfEquations().
 */
int odeSolver::setNumberOfEquations(unsigned n) {
  killAll();

  //Alloker ...
  if (n>0) {
    n_eq = n;
    k1 = new double[n];
    k2 = new double[n];
    k3 = new double[n];
    k4 = new double[n];
    dydt = new double[n];
    y_temp = new double[n];
    y = new double[n];
    next_y = new double[n];
    return 1;
  }
  return 0;
}

/*
  Setter en av y-verdiene, nemlig y[i], til
  value.
  Se også getValue(), getValues(), setValues().
 */
int odeSolver::setValue(unsigned i, double value) {
  if (isInitialized()) {
    if (i<n_eq)
      y[i]=value;
    else {
      //Ugyldig parameter: i>=n_eq.
      return 0;
    }
  } else {
    //Ikke initialisert.
    return 0;
  }
  return 1;
}

/*
  Funksjon for å hente ut en av y-verdiene; nærmere
  bestemt y[i].
  Se også getValues(), setValues(), setValue().
*/
double odeSolver::getValue(unsigned i) {
  if (isInitialized()) {
    if (i<n_eq)
      return y[i];
    else {
      //Ulovlig parameter; i>=n_eq.
      return 0;
    }
  } else {
    //odeSolver ikke initialisert!
    return 0;
  }
  return 1;
}

/*
  Denne funksjonen lar bruker initialisere
  alle y-verdiene ved å peke til en array med
  n_eq double-verdier. Ikke-destruktiv funksjon.
  Se også setValue(), getValue(), setValues(), getValues().
*/
int odeSolver::setValues(const double *allValues) {

  //Avbryt dersom odeSolver ikke er initialisert.
  if (!(isInitialized())) {
      return 0;
  }

  for (unsigned i = 0; i<n_eq; i++) {
    y[i]=allValues[i];
  }
  return 1;
  
}

/*
  Denne funksjonen lar omverdnen lese y[]. allValues
  er en peker til en allerede allokert array som kan holde
  n_eq double-verdier. Nyttig dersom manipulasjon av data trengs.
  Ellers anbefales display() og fdisplay().
  Se også setValues(), getValue(), setValue().
 */
int odeSolver::getValues(double *allValues) {

  //Avbryt dersom odeSolver ikke er initialisert.
  if (!(isInitialized())) {
      return 0;
  }

  for (unsigned i=0; i<n_eq; i++) {
    allValues[i]=y[i];
  }
  return 1;
}

/*
  Skriver ut: t y[0] y[1] ... y[n_eq] til filen
  displayFile. Se også display().
 */
int odeSolver::fdisplay(FILE *displayFile) {
  unsigned i;
 
  fprintf(displayFile, "%e ", t);

  for (i=0; i<n_eq; i++)
    fprintf(displayFile, "%e ", y[i]);

  fprintf(displayFile, "\n");
  return 1;
}
/*
  Kaller rett og slett fdisplay() medstdout som parameter.
 */
int odeSolver::display(void) {
  return fdisplay(stdout);
}

/*
  Dette er selve motoren i ODE-solveren. Algoritmen brukt
  avhenger av variablen algorithm.
  Funksjonen oppdaterer y[] i klasseinstansen, samt tiden t.
  y'ene kan leses med getValues(*double), eller skrives til
  fil (f.eks. stdout) med display()/fdisplay(FILE*).
 */
int odeSolver::calculate(void) {
  unsigned i;
  
  //Sjekk om odeSolver er initialisert.
  if (!(isInitialized())) {
    fprintf(stderr, "odeSolver ikke initialisert!\n");
    return 0;
  }

  if (algorithm==ODE_RK4) {
    //Algoritme for utrgninger av Runke-Kutta av 4. orden. Se journal.
    //Regner ut k1-vektor, så k2-vektor, og så videre, og til
    //slutt next_y-vektor (dvs. y(n+1)) ved hjelp av disse.
    deriv(t, y, dydt);
    for(i=0; i<n_eq; i++){
      k1[i] = h*dydt[i];
      y_temp[i] = y[i] + k1[i]*0.5;
    }
    deriv(t+h*0.5, y_temp, dydt);
    for (i=0; i<n_eq; i++) {
      k2[i] = h*dydt[i];
      y_temp[i] = y[i] + k2[i]*0.5;
    }
    deriv(t+h*0.5, y_temp, dydt);
    for (i=0; i<n_eq; i++) {
      k3[i] = h*dydt[i];
      y_temp[i] = y[i] + k3[i];
    }
    deriv(t+h, y_temp, dydt);
    for (i=0; i<n_eq; i++) {
      k4[i] = h*dydt[i];
    }
    for (i=0; i<n_eq; i++) {
      next_y[i] = y[i] + (1./6.) * (k1[i]+2*k2[i]+2*k3[i]+k4[i]);
      //bare for debugging ...
      ////result[i] = y[i] + (1./6.) * (k1[i]+2*k2[i]+2*k3[i]+k4[i]);
    }

    for (i=0; i<n_eq; i++)
      y[i] = next_y[i];

    //oppdater tid.
    t += h;
  } else if (algorithm==ODE_EULER) {
    deriv(t, y, dydt);
    for(i=0; i<n_eq; i++)
      //Euler: y(n+1) = y(n) + h*f(t, y).
      next_y[i] = y[i] + h*dydt[i];

    //Strengt tatt er det ikke nødvendig å bruke next_y->y, siden
    //utrykkene som avhenger av y[i] (dvs. kn) allerede er evaluert.
    //De ekstra tilordningene vil ikke sinke programmet nevneverdig,
    //og de bidrar til større fleksibilitet ved endringer/debugging.
    //Dette gjelder også ved rk4-algoritmen i forrige blokk.
    for (i=0; i<n_eq; i++)
      y[i] = next_y[i];

    //oppdater tid.
    t+=h;
  }


  //Returner TRUE dersom alt gikk bra.
  return 1;

}

double *odeSolver::directAccess(void) {
  return y;
}