// GetDP - Copyright (C) 1997-2022 P. Dular and C. Geuzaine, University of Liege
//
// See the LICENSE.txt file for license information. Please report all
// issues on https://gitlab.onelab.info/getdp/getdp/issues.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "ProData.h"
#include "DofData.h"
#include "SolvingAnalyse.h"
#include "SolvingOperations.h"
#include "Cal_Quantity.h"
#include "Message.h"

extern struct CurrentData Current;
extern int Init_Update;

#define SQU(a) ((a) * (a))

void Cal_ThetaCoefficients(double *coef)
{
  coef[0] = 1. / Current.DTime;
  coef[1] = Current.Theta;
  coef[2] = -1. / Current.DTime;
  coef[3] = 1. - Current.Theta;
}

void Cal_ThetaMatrix(int *init, double *coef, gMatrix *M1, gMatrix *M2,
                     gMatrix *A)
{
  Message::Info("Generate Theta Iteration Matrix (Theta=%g, DTime=%g)",
                Current.Theta, Current.DTime);

  LinAlg_AssembleMatrix(A);
  LinAlg_ZeroMatrix(A);

  // A = c0 * M2 + c1 * M1
  if(init[2] && coef[0]) LinAlg_AddMatrixProdMatrixDouble(A, M2, coef[0], A);
  if(init[1] && coef[1]) LinAlg_AddMatrixProdMatrixDouble(A, M1, coef[1], A);
}

void Cal_ThetaRHS(int *init, double *coef, gMatrix *M1, gMatrix *M2,
                  gVector *m1, gVector *m2, List_T *m1s, List_T *m2s,
                  gVector *tmp, gVector *b, bool explicitTimeFunction)
{
  double tfval, val;

  LinAlg_ZeroVector(b);

  // b = [-c2 * M2 - c3 * M1 ] * x(n-1)
  if(init[2] && coef[2]) {
    LinAlg_ProdMatrixVector(M2, &(Current.DofData->CurrentSolution - 1)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[2], b);
  }
  if(init[1] && coef[3]) {
    LinAlg_ProdMatrixVector(M1, &(Current.DofData->CurrentSolution - 1)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[3], b);
  }

  if(explicitTimeFunction) {
    //   + [ c0 * m2 + c1 * m1 ] * TimeFct(n)
    tfval = Current.DofData->CurrentSolution->ExplicitTimeFunctionValue;
    if(init[2] && (val = coef[0] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m2, val, b);
    if(init[1] && (val = coef[1] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m1, val, b);

    //   + [ c2 * m2 + c3 * m1 ] * TimeFct(n-1)
    tfval = (Current.DofData->CurrentSolution - 1)->ExplicitTimeFunctionValue;
    if(init[2] && (val = coef[2] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m2, val, b);
    if(init[1] && (val = coef[3] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m1, val, b);
  }
  else {
    for(int i = 0; i < List_Nbr(Current.DofData->TimeFunctionIndex); i++) {
      gVector *mm1 = 0, *mm2 = 0;
      if(init[1]) mm1 = (gVector *)List_Pointer(m1s, i);
      if(init[2]) mm2 = (gVector *)List_Pointer(m2s, i);

      int tfindex;
      List_Read(Current.DofData->TimeFunctionIndex, i, &tfindex);

      //   + [ c0 * m2 + c1 * m1 ] * TimeFct(n)
      tfval = Current.DofData->CurrentSolution->TimeFunctionValues[tfindex];
      if(init[2] && (val = coef[0] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm2, val, b);
      if(init[1] && (val = coef[1] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm1, val, b);

      //   + [ c2 * m2 + c3 * m1 ] * TimeFct(n-1)
      tfval =
        (Current.DofData->CurrentSolution - 1)->TimeFunctionValues[tfindex];
      if(init[2] && (val = coef[2] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm2, val, b);
      if(init[1] && (val = coef[3] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm1, val, b);
    }
  }
}

void Cal_NewmarkCoefficients(double *coef)
{
  coef[0] = 1. / SQU(Current.DTime);
  coef[1] = Current.Gamma / Current.DTime;
  coef[2] = Current.Beta;
  coef[3] = -2. / SQU(Current.DTime);
  coef[4] = (1. - 2. * Current.Gamma) / Current.DTime;
  coef[5] = 0.5 + Current.Gamma - 2. * Current.Beta;
  coef[6] = 1. / SQU(Current.DTime);
  coef[7] = (Current.Gamma - 1.) / Current.DTime;
  coef[8] = 0.5 - Current.Gamma + Current.Beta;
}

void Cal_NewmarkMatrix(int *init, double *coef, gMatrix *M1, gMatrix *M2,
                       gMatrix *M3, gMatrix *A)
{
  Message::Info(
    "Generate Newmark Iteration Matrix (Beta=%g, Gamma=%g, DTime=%g)",
    Current.Beta, Current.Gamma, Current.DTime);

  LinAlg_AssembleMatrix(A);
  LinAlg_ZeroMatrix(A);

  // A = c0 * M3 + c1 * M2 + c2 * M3
  if(init[3] && coef[0]) LinAlg_AddMatrixProdMatrixDouble(A, M3, coef[0], A);
  if(init[2] && coef[1]) LinAlg_AddMatrixProdMatrixDouble(A, M2, coef[1], A);
  if(init[1] && coef[2]) LinAlg_AddMatrixProdMatrixDouble(A, M1, coef[2], A);
}

void Cal_NewmarkRHS(int *init, double *coef, gMatrix *M1, gMatrix *M2,
                    gMatrix *M3, gVector *m1, gVector *m2, gVector *m3,
                    List_T *m1s, List_T *m2s, List_T *m3s, gVector *tmp,
                    gVector *b, bool explicitTimeFunction)
{
  double tfval, val;

  LinAlg_ZeroVector(b);

  // b = [-c3 * M3 - c4 * M2 - c5 * M1] * x(n-1)
  if(init[3] && coef[3]) {
    LinAlg_ProdMatrixVector(M3, &(Current.DofData->CurrentSolution - 1)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[3], b);
  }
  if(init[2] && coef[4]) {
    LinAlg_ProdMatrixVector(M2, &(Current.DofData->CurrentSolution - 1)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[4], b);
  }
  if(init[1] && coef[5]) {
    LinAlg_ProdMatrixVector(M1, &(Current.DofData->CurrentSolution - 1)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[5], b);
  }

  //   + [-c6 * M3 - c7 * M2 - c8 * M1] * x(n-2)
  if(init[3] && coef[6]) {
    LinAlg_ProdMatrixVector(M3, &(Current.DofData->CurrentSolution - 2)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[6], b);
  }
  if(init[2] && coef[7]) {
    LinAlg_ProdMatrixVector(M2, &(Current.DofData->CurrentSolution - 2)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[7], b);
  }
  if(init[1] && coef[8]) {
    LinAlg_ProdMatrixVector(M1, &(Current.DofData->CurrentSolution - 2)->x,
                            tmp);
    LinAlg_AddVectorProdVectorDouble(b, tmp, -coef[8], b);
  }

  if(explicitTimeFunction) {
    //   + [ c0 * m3 + c1 * m2 + c2 * m1 ] * TimeFct(n)
    tfval = Current.DofData->CurrentSolution->ExplicitTimeFunctionValue;
    if(init[3] && (val = coef[0] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m3, val, b);
    if(init[2] && (val = coef[1] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m2, val, b);
    if(init[1] && (val = coef[2] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m1, val, b);

    //   + [ c3 * m3 + c4 * m2 + c5 * m1 ] * TimeFct(n-1)
    tfval = (Current.DofData->CurrentSolution - 1)->ExplicitTimeFunctionValue;
    if(init[3] && (val = coef[3] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m3, val, b);
    if(init[2] && (val = coef[4] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m2, val, b);
    if(init[1] && (val = coef[5] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m1, val, b);

    //   + [ c6 * m3 + c7 * m2 + c8 * m1 ] * TimeFct(n-2)
    tfval = (Current.DofData->CurrentSolution - 2)->ExplicitTimeFunctionValue;
    if(init[3] && (val = coef[6] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m3, val, b);
    if(init[2] && (val = coef[7] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m2, val, b);
    if(init[1] && (val = coef[8] * tfval))
      LinAlg_AddVectorProdVectorDouble(b, m1, val, b);
  }
  else {
    for(int i = 0; i < List_Nbr(Current.DofData->TimeFunctionIndex); i++) {
      gVector *mm1 = 0, *mm2 = 0, *mm3 = 0;
      if(init[1]) mm1 = (gVector *)List_Pointer(m1s, i);
      if(init[2]) mm2 = (gVector *)List_Pointer(m2s, i);
      if(init[3]) mm3 = (gVector *)List_Pointer(m3s, i);

      int tfindex;
      List_Read(Current.DofData->TimeFunctionIndex, i, &tfindex);

      //   + [ c0 * m3 + c1 * m2 + c2 * m1 ] * TimeFct(n)
      tfval = Current.DofData->CurrentSolution->TimeFunctionValues[tfindex];

      if(init[3] && (val = coef[0] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm3, val, b);
      if(init[2] && (val = coef[1] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm2, val, b);
      if(init[1] && (val = coef[2] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm1, val, b);

      //   + [ c3 * m3 + c4 * m2 + c5 * m1 ] * TimeFct(n-1)
      tfval =
        (Current.DofData->CurrentSolution - 1)->TimeFunctionValues[tfindex];

      if(init[3] && (val = coef[3] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm3, val, b);
      if(init[2] && (val = coef[4] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm2, val, b);
      if(init[1] && (val = coef[5] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm1, val, b);

      //   + [ c6 * m3 + c7 * m2 + c8 * m1 ] * TimeFct(n-2)
      tfval =
        (Current.DofData->CurrentSolution - 2)->TimeFunctionValues[tfindex];

      if(init[3] && (val = coef[6] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm3, val, b);
      if(init[2] && (val = coef[7] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm2, val, b);
      if(init[1] && (val = coef[8] * tfval))
        LinAlg_AddVectorProdVectorDouble(b, mm1, val, b);
    }
  }
}

void Operation_Update(struct DefineSystem *DefineSystem_P,
                      struct DofData *DofData_P, struct DofData *DofData_P0,
                      int TimeFunctionIndex)
{
  int i, i_TimeStep;
  struct Solution *Solution_P, Solution_S;
  struct Value Value;

  static gVector TmpVect;
  static double coef[9];
  static double Save_Num, Save_DTime, Save_Theta, Save_Beta, Save_Gamma;

  if(!DofData_P->Solutions)
    Message::Error("No initialized solution available for update");

  i_TimeStep = (int)Current.TimeStep;
  if(!(Solution_P = (struct Solution *)List_PQuery(DofData_P->Solutions,
                                                   &i_TimeStep, fcmp_int))) {
    Solution_S.TimeStep = (int)Current.TimeStep;
    Solution_S.Time = Current.Time;
    Solution_S.TimeImag = Current.TimeImag;
    Solution_S.TimeFunctionValues = Get_TimeFunctionValues(DofData_P);

    if(TimeFunctionIndex >= 0) {
      Get_ValueOfExpressionByIndex(TimeFunctionIndex, NULL, 0., 0., 0., &Value);
      Solution_S.ExplicitTimeFunctionValue = Value.Val[0];
    }

    Solution_S.SolutionExist = 1;
    LinAlg_CreateVector(&Solution_S.x, &DofData_P->Solver, DofData_P->NbrDof);
    LinAlg_ZeroVector(&Solution_S.x);
    List_Add(DofData_P->Solutions, &Solution_S);
    DofData_P->CurrentSolution = (struct Solution *)List_Pointer(
      DofData_P->Solutions, List_Nbr(DofData_P->Solutions) - 1);
  }
  else if(Solution_P != DofData_P->CurrentSolution) {
    Message::Error("Incompatible time");
  }

  switch(Current.TypeTime) {
  case TIME_THETA:

    if(!DofData_P->Flag_Init[1] && !DofData_P->Flag_Init[2])
      Message::Error("No system available for Update");

    if(!Init_Update) {
      Init_Update = 1;

      /* bidouillage provisoire : a revoir qd les conditions initiales multiples
         seront mieux traitees */
      Current.Time -= Current.DTime;
      Current.TimeStep -= 1.;
      if(TimeFunctionIndex >= 0) {
        Get_ValueOfExpressionByIndex(TimeFunctionIndex, NULL, 0., 0., 0.,
                                     &Value);
        (DofData_P->CurrentSolution - 1)->ExplicitTimeFunctionValue =
          Value.Val[0];
      }
      Current.Time += Current.DTime;
      Current.TimeStep += 1.;
      /* */

      LinAlg_CreateVector(&TmpVect, &DofData_P->Solver, DofData_P->NbrDof);

      Save_Num = DofData_P->Num;
      Save_DTime = Current.DTime;
      Save_Theta = Current.Theta;

      Cal_ThetaCoefficients(coef);
      Cal_ThetaMatrix(DofData_P->Flag_Init, coef, &DofData_P->M1,
                      &DofData_P->M2, &DofData_P->A);
      LinAlg_AssembleMatrix(&DofData_P->A);
    }

    if(Save_Num != DofData_P->Num || Current.DTime != Save_DTime ||
       Current.Theta != Save_Theta) {
      Save_Num = DofData_P->Num;
      Save_DTime = Current.DTime;
      Save_Theta = Current.Theta;
      Cal_ThetaCoefficients(coef);
      Cal_ThetaMatrix(DofData_P->Flag_Init, coef, &DofData_P->M1,
                      &DofData_P->M2, &DofData_P->A);
      LinAlg_AssembleMatrix(&DofData_P->A);
    }

    Cal_ThetaRHS(DofData_P->Flag_Init, coef, &DofData_P->M1, &DofData_P->M2,
                 &DofData_P->m1, &DofData_P->m2, DofData_P->m1s, DofData_P->m2s,
                 &TmpVect, &DofData_P->b, (TimeFunctionIndex >= 0));
    LinAlg_AssembleVector(&DofData_P->b);
    break;

  case TIME_NEWMARK:

    if(!DofData_P->Flag_Init[1] && !DofData_P->Flag_Init[2] &&
       !DofData_P->Flag_Init[3])
      Message::Error("No system available for Update");

    if(!Init_Update) {
      Init_Update = 1;

      /* bidouillage provisoire : a revoir qd les conditions initiales multiples
         seront mieux traitees */
      Current.Time -= Current.DTime;
      Current.TimeStep -= 1.;
      if(TimeFunctionIndex >= 0) {
        Get_ValueOfExpressionByIndex(TimeFunctionIndex, NULL, 0., 0., 0.,
                                     &Value);
        (DofData_P->CurrentSolution - 1)->ExplicitTimeFunctionValue =
          Value.Val[0];
        (DofData_P->CurrentSolution - 2)->ExplicitTimeFunctionValue =
          Value.Val[0];
      }
      Current.Time += Current.DTime;
      Current.TimeStep += 1.;
      /* */

      LinAlg_CreateVector(&TmpVect, &DofData_P->Solver, DofData_P->NbrDof);

      Save_Num = DofData_P->Num;
      Save_DTime = Current.DTime;
      Save_Beta = Current.Beta;
      Save_Gamma = Current.Gamma;
      Cal_NewmarkCoefficients(coef);
      Cal_NewmarkMatrix(DofData_P->Flag_Init, coef, &DofData_P->M1,
                        &DofData_P->M2, &DofData_P->M3, &DofData_P->A);
      LinAlg_AssembleMatrix(&DofData_P->A);
    }

    if(Save_Num != DofData_P->Num || Current.DTime != Save_DTime ||
       Current.Beta != Save_Beta || Current.Gamma != Save_Gamma) {
      Save_Num = DofData_P->Num;
      Save_DTime = Current.DTime;
      Save_Beta = Current.Beta;
      Save_Gamma = Current.Gamma;
      Cal_NewmarkCoefficients(coef);
      Cal_NewmarkMatrix(DofData_P->Flag_Init, coef, &DofData_P->M1,
                        &DofData_P->M2, &DofData_P->M3, &DofData_P->A);
      LinAlg_AssembleMatrix(&DofData_P->A);
    }

    Cal_NewmarkRHS(DofData_P->Flag_Init, coef, &DofData_P->M1, &DofData_P->M2,
                   &DofData_P->M3, &DofData_P->m1, &DofData_P->m2,
                   &DofData_P->m3, DofData_P->m1s, DofData_P->m2s,
                   DofData_P->m3s, &TmpVect, &DofData_P->b,
                   (TimeFunctionIndex >= 0));
    LinAlg_AssembleVector(&DofData_P->b);
    break;

  default: Message::Error("Wrong type of analysis for update");
  }

  LinAlg_GetVectorSize(&DofData_P->b, &i);
  if(!i) Message::Info("Generated system is of dimension zero");

  Free_UnusedSolutions(DofData_P);
}
