#include "MemoryManager.h"
#include "spnbox.h"
#include "matrixmath.h"
#include "../pnheaders/matrix.h"
#include "../pnheaders/pns.h"

/*Convert disjunctive constraints to normal constraints, get new D L b m0*/
int disq2disj(pns* plant1, matrix *L, matrix* H, pns* plant2, matrix* L_star)
{
  pns Nstar[L->nr+1], ng;
  matrix D, l, lD, h, hd, l_g, L_g, L_g_nonzero;
  int i, j, max;
  int Ng_label[H->nc], Ng_label_nonzero;
  int* ng_label;
  int ng_D[] = {1, -1};
  int ng_m0[] = {0};
  int ng_Tlabel[] = {0, 0};
  MemoryManager memory;
  memory = CreateMemoryManager(20, 20, 0, 0);
  
  for(j = 0; j < H->nc; j++)
    Ng_label[j] = 0;
  MAllocateMatrixType(&memory, 2, &D, plant1->in.nr, plant1->in.nc);
  SubtractMatrix(&plant1->in, &plant1->out, &D);
  Nstar[0] = copypn(plant1);
  MAllocateMatrixType(&memory, 2, &l, 1, L->nc);
  MAllocateMatrixType(&memory, 2, &h, 1, H->nc);
  MAllocateMatrixType(&memory, 2, &lD, 1, H->nc);
  MAllocateMatrixType(&memory, 2, &hd, 1, H->nc);
  MAllocateMatrixType(&memory, 2, &l_g, 1, H->nc);
  MAllocateMatrixType(&memory, 2, &L_g, L->nr, H->nc);

  for(i = 0; i < L->nr; i++){
    CopyBlock(1, L->nc, L, i, 0, &l, 0, 0);
    CopyBlock(1, H->nc, H, i, 0, &h, 0, 0);
    MultiplyMatrix(&l, &D, &lD);
    for(j = 0; j < hd.nc; j++){
      max = 0;
      if(GetMatrixEl(&lD, 0, j) > max) 
	max = GetMatrixEl(&lD, 0, j);
      if(GetMatrixEl(&h, 0, j) > max) 
	max = GetMatrixEl(&h, 0, j);
      SetMatrixEl(&hd, 0, j, max);
      if(max != GetMatrixEl(&lD, 0, j))
	Ng_label[j] = 1;
    }
    SubtractMatrix(&hd, &lD, &l_g);
    CopyBlock(1, H->nc, &l_g, 0, 0, &L_g, i, 0);
  }
  
  for(j = 0, Ng_label_nonzero = 0; j < H->nc; j++)
    if(Ng_label[j] != 0)
      Ng_label_nonzero++;
  MAllocateMatrixType(&memory, 2, &L_g_nonzero, L->nr, Ng_label_nonzero);
  for(j = 0, Ng_label_nonzero = 0; j < H->nc; j++)
    if(Ng_label[j] != 0) {
      CopyBlock(L->nr, 1, &L_g, 0, j, &L_g_nonzero, 0, Ng_label_nonzero);
      Ng_label_nonzero++;
    }
  AllocateMatrix(L_star, L->nr, L->nc + L_g_nonzero.nc);
  CopyBlock(L->nr, L->nc, L, 0, 0, L_star, 0, 0);
  CopyBlock(L_g_nonzero.nr, L_g_nonzero.nc, &L_g_nonzero, 0, 0, L_star, 0, L->nc);

  ng = createpn("pnum tnum D m0 label", 1, 2, ng_D, ng_m0, ng_Tlabel);
  for(j = 0, Ng_label_nonzero = 0; j < H->nc; j++)
    if(Ng_label[j] != 0) {
      ng_Tlabel[0] = j + 1;
      ng_Tlabel[1] = j + 1 + D.nc;
      updatepn(&ng, "label", ng_Tlabel);      
      Nstar[Ng_label_nonzero + 1] = composepn(&Nstar[Ng_label_nonzero], &ng);
      Ng_label_nonzero++;
    }
  
  *plant2 = copypn(&Nstar[Ng_label_nonzero]);

  for(i = 0; i < Ng_label_nonzero; i++)
    deallocpn(&Nstar[i]);
  deallocpn(&ng);
  FreeMemory(&memory);
  printf("Q_Disjunctive constraints converted.\n");
  return 0;
}

