/*
  Copyright(C) 2007-2012 National Institute of Information and Communications Technology
*/

/*
  svmtools
  Kernel module
*/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "exception.h"
#include "svm_common.h"
#include "svm_kernel.h"


#define CACHEMASK (UINT_MAX / 2U + 1U)	/* アクセスされたことを示すフラグ */


static void kernel_linear(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_rbf(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_sigmoid(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial_1o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial_2o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial_3o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial_4o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void kernel_polynomial_5o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *x, float *q);
static void dotprod(SVM_KWRK *kwrk, SVM_SV *x, float *q);


/* カーネルの関数ポインタ(内積のベクトルを入力してカーネルのベクトルを出力) */
static void (*kernel[])(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) = {
  kernel_linear,
  kernel_polynomial,
  kernel_rbf,
  kernel_sigmoid,
  kernel_polynomial_1o,
  kernel_polynomial_2o,
  kernel_polynomial_3o,
  kernel_polynomial_4o,
  kernel_polynomial_5o,
};


/*
  キャッシュの初期化
*/
SVM_KC *svm_knl_kcinit(SVM_KPRM *kprm, int num, int *label, SVM_SV **sv, int size) {
  int i;
  SVM_KC *kc;

  kc = smalloc(sizeof(SVM_KC));
  kc->kprm = *kprm;
  kc->col = num;
  kc->row = (size * 1024 * 1024) / (sizeof(float) * kc->col);
  if (kc->row < 2) kc->row = 2;
  if (kc->row > num) kc->row = num;
  kc->size = kc->col * kc->row;
  kc->cache = smalloc(sizeof(float) * kc->col * kc->row);
  kc->e2r = smalloc(sizeof(int) * num);
  for (i = 0; i < num; i++) kc->e2r[i] = -1;
  kc->r2e = smalloc(sizeof(unsigned int) * num);
  for (i = 0; i < num; i++) kc->r2e[i] = 0U;
  kc->nxt = 0;
  kc->gen = smalloc(sizeof(int) * num);
  kc->hst_num = 0;
  kc->hst = smalloc(sizeof(int) * num);
  kc->label = smalloc(sizeof(int) * num);
  for (i = 0; i < num; i++) kc->label[i] = label[i];

  /* 作業用データの初期化 */
  kc->kwrk = svm_knl_kwrkinit(kprm, num, sv);

  return kc;
}


/*
  キャッシュ情報の削除
*/
void svm_knl_kcdelete(SVM_KC *kc) {
  svm_knl_kwrkdelete(kc->kwrk);
  free(kc->label);
  free(kc->hst);
  free(kc->gen);
  free(kc->r2e);
  free(kc->e2r);
  free(kc->cache);
  free(kc);
}


/*
  キャッシュを経由してカーネル計算
*/
float *svm_knl_kckernel(SVM_KC *kc, int p, float *rsv) {
  int i, j;
  int row;
  float *cache;
  int *l, *l_end;
  float *c;

  row = kc->e2r[p];
  if (row != -1) {	/* cache hit */
    kc->r2e[row] |= CACHEMASK;	/* フラグをセット */
    cache = kc->cache + kc->col * row;

    /* 世代の調整(穴を埋める) */
    for (i = kc->gen[row], j = kc->kwrk->num + (kc->hst_num - kc->gen[row]) - 1; i < kc->hst_num; i++, j--) {
      cache[kc->hst[i]] = cache[j];
    }
    kc->gen[row] = kc->hst_num;

    return cache;
  } else {	/* cache miss */
    /* 捨てるキャッシュを見付ける(Clock algorithm) */
    for (row = kc->nxt; ; row++) {
      if (row >= kc->row) row = 0;
      if ((kc->r2e[row] & CACHEMASK) == 0U && kc->cache + kc->col * row != rsv) break;
      /* フラグをリセット */
      kc->r2e[row] &= ~CACHEMASK;
    }
    cache = kc->cache + kc->col * row;
    kc->nxt = row + 1;
    if (kc->r2e[row] < (unsigned int)kc->kwrk->num && kc->e2r[kc->r2e[row]] == row) kc->e2r[kc->r2e[row]] = -1;	/* kc->r2e[row] >= 0 */
    kc->e2r[p] = row;
    kc->r2e[row] = (unsigned int)p;
    kc->gen[row] = kc->hst_num;

    /* sqnormの計算 */
    if (kc->kprm.ktype == SVM_KERNEL_RBF) {
      kc->kwrk->sqnorm2 = kc->kwrk->sqnorm[p];
    }

    /* 新規計算 */
    (kernel[kc->kwrk->kfunc])(&kc->kprm, kc->kwrk, kc->kwrk->sv[p], cache);
    if (kc->label[p] > 0) {
      for (l = kc->label, l_end = l + kc->kwrk->num, c = cache; l < l_end; l++, c++) if (*l < 0) *c = -*c;
    } else {
      for (l = kc->label, l_end = l + kc->kwrk->num, c = cache; l < l_end; l++, c++) if (*l > 0) *c = -*c;
    }

    return cache;
  }
}


/*
  指定された事例idをキャッシュ上で無効にする
*/
void svm_knl_kchide(SVM_KC *kc, int p) {
  /* 無効になったインデックスをヒストリに記録 */
  kc->hst[kc->hst_num++] = p;

  /* その他の移動 */
  if (kc->kwrk->sqnorm != NULL) kc->kwrk->sqnorm[p] = kc->kwrk->sqnorm[kc->kwrk->num - 1];
  kc->kwrk->sv[p] = kc->kwrk->sv[kc->kwrk->num - 1];
  kc->label[p] = kc->label[kc->kwrk->num - 1];
  kc->e2r[p] = kc->e2r[kc->kwrk->num - 1];
  if (kc->e2r[p] != -1) {
    kc->r2e[kc->e2r[p]] &= CACHEMASK;
    kc->r2e[kc->e2r[p]] |= (unsigned int)p;
  }
  kc->kwrk->num--;

  return;
}

/*
  キャッシュを増やす
*/
void svm_knl_kcexpand(SVM_KC *kc) {
  int i, j, k;

  if ((kc->col - kc->kwrk->num) * kc->row >= kc->kwrk->num && kc->row < kc->kwrk->num) {
    for (i = 0; i < kc->row; i++) {
      float *cache;

      if (kc->e2r[kc->r2e[i] & ~CACHEMASK] != i) continue;	/* 未使用 */

      cache = kc->cache + kc->col * i;
      /* 世代の調整(穴を埋める) */
      for (j = kc->gen[i], k = kc->kwrk->num + (kc->hst_num - kc->gen[i]) - 1; j < kc->hst_num; j++, k--) {
	cache[kc->hst[j]] = cache[k];
      }
      kc->gen[i] = kc->hst_num;

      /* 移動 */
      if (i != 0) memmove(kc->cache + kc->kwrk->num * i, cache, sizeof(float) * kc->kwrk->num);
    }
    kc->col = kc->kwrk->num;
    kc->row = kc->size / kc->col;
    if (kc->row > kc->kwrk->num) kc->row = kc->kwrk->num;
  }

  return;
}


/*
  キャッシュの作業用情報の初期化
*/
SVM_KWRK *svm_knl_kwrkinit(SVM_KPRM *kprm, int num, SVM_SV **sv) {
  int i, j;
  SVM_KWRK *kwrk;
  int max;

  kwrk = smalloc(sizeof(SVM_KWRK));

  if (kprm->ktype == SVM_KERNEL_POLYNOMIAL) {
    if (kprm->degree == 1.0f) kwrk->kfunc = SVM_KFUNC_POLYNOMIAL_1O;
    else if (kprm->degree == 2.0f) kwrk->kfunc = SVM_KFUNC_POLYNOMIAL_2O;
    else if (kprm->degree == 3.0f) kwrk->kfunc = SVM_KFUNC_POLYNOMIAL_3O;
    else if (kprm->degree == 4.0f) kwrk->kfunc = SVM_KFUNC_POLYNOMIAL_4O;
    else if (kprm->degree == 5.0f) kwrk->kfunc = SVM_KFUNC_POLYNOMIAL_5O;
    else kwrk->kfunc = kprm->ktype;
  } else {
    kwrk->kfunc = kprm->ktype;
  }

  kwrk->num = num;
  if (num == 0) {
    kwrk->sv = NULL;
    kwrk->sqnorm = NULL;
    kwrk->max = 0;
    kwrk->tbl = NULL;
    return kwrk;
  }
  kwrk->sv = smalloc(sizeof(SVM_SV *) * num);
  for (i = 0; i < num; i++) kwrk->sv[i] = sv[i];

  /* ノルムの自乗の計算 */
  if (kprm->ktype == SVM_KERNEL_RBF) {	/* RBF kernel */
    kwrk->sqnorm = smalloc(sizeof(float) * num);
    if (sv[0]->val != NULL) {	/* real-valued vector */
      for (i = 0; i < num; i++) {
	kwrk->sqnorm[i] = 0.0f;
	for (j = 0; j < sv[i]->num; j++) kwrk->sqnorm[i] += sv[i]->val[j] * sv[i]->val[j];
      }
    } else {	/* binary-valued vector */
      for (i = 0; i < num; i++) {
	kwrk->sqnorm[i] = (float)sv[i]->num;
      }
    }
  } else {
    kwrk->sqnorm = NULL;
  }

  max = 1;
  for (i = 0; i < num; i++) {
    for (j = 0; j < sv[i]->num; j++) {
      if (max < sv[i]->idx[j]) max = sv[i]->idx[j];
    }
  }
  max++;
  kwrk->max = max;
  kwrk->tbl = smalloc(sizeof(int) * (max + 1));	/* テーブルは(max +1)個確保する．索引表を使った内積計算では最後の1個は不要(なので初期化も必要ない)．転置インデックスを作る場合，n番目の素性の開始位置がtbl[n]で終了位置がtbl[n+1]となるため，最後の素性の終了位置を保持するために1つ余分な要素が必要となる． */
  for (i = 0; i < max; i++) kwrk->tbl[i] = -1;

  return kwrk;
}


/*
  キャッシュの作業用情報の初期化(転置ベクトル版)
*/
SVM_KWRK *svm_knl_kwrkinit_inv(SVM_KPRM *kprm, int num, SVM_SV **sv) {
  int i, j, k;
  SVM_KWRK *kwrk;
  int sum;
  int *tmp_eid;
  float *tmp_val;
  int *tmp_nxt;

  /* 共通部分初期化 */
  kwrk = svm_knl_kwrkinit(kprm, num, sv);
  if (num == 0) return kwrk;
  free(kwrk->sv);
  kwrk->sv = NULL;

  /* 転置インデックスの作成 */
  sum = 0;
  for (i = 0; i < num; i++) sum += sv[i]->num;
  tmp_nxt = smalloc(sizeof(int) * sum);
  tmp_eid = smalloc(sizeof(int) * sum);
  k = 0;
  for (i = 0; i < num; i++) {
    for (j = 0; j < sv[i]->num; j++) {
      tmp_eid[k] = i;
      tmp_nxt[k] = kwrk->tbl[sv[i]->idx[j]];
      kwrk->tbl[sv[i]->idx[j]] = k;
      k++;
    }
  }
  if (sv[0]->val == NULL) {
    tmp_val = NULL;
  } else {
    tmp_val = smalloc(sizeof(float) * sum);
    k = 0;
    for (i = 0; i < num; i++) {
      memcpy(tmp_val + k, sv[i]->val, sizeof(float) * sv[i]->num);
      k += sv[i]->num;
    }
  }

  /* 整列 */
  if (tmp_val != NULL) {
    kwrk->inv_eid = smalloc(sizeof(int) * sum);
    kwrk->inv_val = smalloc(sizeof(float) * sum);
    k = 0;
    for (i = 0; i < kwrk->max; i++) {
      for (j = kwrk->tbl[i], kwrk->tbl[i] = k; j != -1; j = tmp_nxt[j]) {
	kwrk->inv_eid[k] = tmp_eid[j];
	kwrk->inv_val[k] = tmp_val[j];
	k++;
      }
    }
    kwrk->tbl[i] = k;
    free(tmp_eid);
    free(tmp_val);
    free(tmp_nxt);
  } else {
    kwrk->inv_eid = smalloc(sizeof(int) * sum);
    kwrk->inv_val = NULL;
    k = 0;
    for (i = 0; i < kwrk->max; i++) {
      for (j = kwrk->tbl[i], kwrk->tbl[i] = k; j != -1; j = tmp_nxt[j]) {
	kwrk->inv_eid[k] = tmp_eid[j];
	k++;
      }
    }
    kwrk->tbl[i] = k;
    free(tmp_eid);
    free(tmp_nxt);
  }

  return kwrk;
}


/*
  キャッシュの作業用情報の削除
*/
void svm_knl_kwrkdelete(SVM_KWRK *kwrk) {
  if (kwrk->tbl != NULL) free(kwrk->tbl);
  if (kwrk->sqnorm != NULL) free(kwrk->sqnorm);
  if (kwrk->sv != NULL) {
    free(kwrk->sv);
  } else {
    free(kwrk->inv_eid);
    if (kwrk->inv_val != NULL) free(kwrk->inv_val);
  }
  free(kwrk);

  return;
}


/*
  カーネルの計算
*/
void svm_knl_kwrkkernel(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  /* sqnormの計算 */
  if (kprm->ktype == SVM_KERNEL_RBF) {
    if (v->val == NULL) {	/* binary-valued vector */
      kwrk->sqnorm2 = (float)v->num;
    } else {	/* real-valued vector */
      float *val, *val_end;

      kwrk->sqnorm2 = 0.0f;
      for (val = v->val, val_end = v->val + v->num; val < val_end; val++) kwrk->sqnorm2 += *val * *val;
    }
  }

  (kernel[kwrk->kfunc])(kprm, kwrk, v, q);

  return;
}


/*
  Linear kernel
*/
static void kernel_linear(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  dotprod(kwrk, v, q);

  return;
}


/*
  Polynomial kernel
*/
static void kernel_polynomial(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    *q = powf(kprm->gamma * *q + kprm->coef, kprm->degree);
  }

  return;
}


/*
  Radial Basis Function kernel
*/
static void kernel_rbf(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;
  float *sqnorm;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num, sqnorm = kwrk->sqnorm; q < q_end; q++, sqnorm++) {
    *q = expf(-kprm->gamma * (*sqnorm + kwrk->sqnorm2 - (*q + *q)));
  }

  return;
}


/*
  Sigmoid kernel
*/
static void kernel_sigmoid(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    *q = tanhf(kprm->gamma * *q + kprm->coef);
  }

  return;
}


/*
  Polynomial kernel(1st order)
*/
static void kernel_polynomial_1o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    *q = kprm->gamma * *q + kprm->coef;
  }

  return;
}


/*
  Polynomial kernel(2nd order)
*/
static void kernel_polynomial_2o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;
  float r;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    r = kprm->gamma * *q + kprm->coef;
    *q = r * r;
  }

  return;
}


/*
  Polynomial kernel(3rd order)
*/
static void kernel_polynomial_3o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;
  float r;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    r = kprm->gamma * *q + kprm->coef;
    *q = r * r * r;
  }

  return;
}


/*
  Polynomial kernel(4th order)
*/
static void kernel_polynomial_4o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;
  float r;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    r = kprm->gamma * *q + kprm->coef;
    *q = r * r * r * r;
  }

  return;
}


/*
  Polynomial kernel(5th order)
*/
static void kernel_polynomial_5o(SVM_KPRM *kprm, SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  float *q_end;
  float r;

  dotprod(kwrk, v, q);

  for (q_end = q + kwrk->num; q < q_end; q++) {
    r = kprm->gamma * *q + kprm->coef;
    *q = r * r * r * r * r;
  }

  return;
}


/*
  内積の計算
*/
static void dotprod(SVM_KWRK *kwrk, SVM_SV *v, float *q) {
  int i, j;
  SVM_SV **sv;
  float *q_end;
  int *vidx, *vidx_end;
  SVM_SV *w;
  int *widx, *widx_end;
  int max;

  if (v->num == 0) {
    memset(q, 0x00, sizeof(float) * kwrk->num);	/* 0に初期化(IEEE 754) */
    return;
  }

  if (kwrk->sv == NULL) {	/* 転置ベクトルによる計算 */
    memset(q, 0x00, sizeof(float) * kwrk->num);	/* 0に初期化(IEEE 754) */

    if (v->val == NULL) {	/* input=binary-valued vector */
      if (kwrk->inv_val == NULL) {	/* target=binary-valued vector */
	for (i = 0; i < v->num; i++) {
	  int *eid, *eid_end;
	  if (v->idx[i] >= kwrk->max) break;
	  for (eid = kwrk->inv_eid + kwrk->tbl[v->idx[i]], eid_end = kwrk->inv_eid + kwrk->tbl[v->idx[i] + 1]; eid < eid_end; eid++) {
	    q[*eid] += 1.0f;
	  }
	}
      } else {	/* target=real-valued vector */
	for (i = 0; i < v->num; i++) {
	  int *eid, *eid_end;
	  float *val;
	  if (v->idx[i] >= kwrk->max) break;
	  for (eid = kwrk->inv_eid + kwrk->tbl[v->idx[i]], eid_end = kwrk->inv_eid + kwrk->tbl[v->idx[i] + 1], val = kwrk->inv_val + kwrk->tbl[v->idx[i]]; eid < eid_end; eid++, val++) {
	    q[*eid] += *val;
	  }
	}
      }
    } else {	/* input=real-valued vector */
      if (kwrk->inv_val == NULL) {	/* target=binary-valued vector */
	for (i = 0; i < v->num; i++) {
	  int *eid, *eid_end;
	  if (v->idx[i] >= kwrk->max) break;
	  for (eid = kwrk->inv_eid + kwrk->tbl[v->idx[i]], eid_end = kwrk->inv_eid + kwrk->tbl[v->idx[i] + 1]; eid < eid_end; eid++) {
	    q[*eid] += v->val[i];
	  }
	}
      } else {	/* target=real-valued vector */
	for (i = 0; i < v->num; i++) {
	  int *eid, *eid_end;
	  float *val;
	  if (v->idx[i] >= kwrk->max) break;
	  for (eid = kwrk->inv_eid + kwrk->tbl[v->idx[i]], eid_end = kwrk->inv_eid + kwrk->tbl[v->idx[i] + 1], val = kwrk->inv_val + kwrk->tbl[v->idx[i]]; eid < eid_end; eid++, val++) {
	    q[*eid] += *val * v->val[i];
	  }
	}
      }
    }

    return;
  }

  /* 索引表の準備 */
  for (vidx = v->idx, vidx_end = vidx + v->num, i = 0; vidx < vidx_end; vidx++, i++) {
    if (*vidx >= kwrk->max) break;
    kwrk->tbl[*vidx] = i;
  }
  max = (i > 0) ? v->idx[i - 1] : -1;

  /* 内積の計算 */
  if (v->val == NULL) {	/* input=binary-valued vector */
    if (kwrk->sv[0]->val == NULL) {	/* target=binary-valued vector */
      for (q_end = q + kwrk->num, sv = kwrk->sv; q < q_end; q++, sv++) {
	int qq;

	w = *sv;
	if (w->num == 0) {
	  *q = 0.0f;
	  continue;
	}
	qq = 0;
	for (widx = w->idx, widx_end = widx + w->num; widx < widx_end; widx++) {
	  if (*widx > max) break;
	  j = kwrk->tbl[*widx];
	  if (j != -1) {
	    qq++;
	  }
	}
	*q = (float)qq;
      }
    } else {	/* target=real-valued vector */
      for (q_end = q + kwrk->num, sv = kwrk->sv; q < q_end; q++, sv++) {
	float qq;

	w = *sv;
	if (w->num == 0) {
	  *q = 0.0f;
	  continue;
	}
	qq = 0.0f;
	for (widx = w->idx, widx_end = widx + w->num; widx < widx_end; widx++) {
	  if (*widx > max) break;
	  j = kwrk->tbl[*widx];
	  if (j != -1) {
	    qq += w->val[widx - w->idx];
	  }
	}
	*q = qq;
      }
    }
  } else {	/* input=real-valued vector */
    if (kwrk->sv[0]->val == NULL) {	/* target=binary-valued vector */
      for (q_end = q + kwrk->num, sv = kwrk->sv; q < q_end; q++, sv++) {
	float qq;

	w = *sv;
	if (w->num == 0) {
	  *q = 0.0f;
	  continue;
	}
	qq = 0.0f;
	for (widx = w->idx, widx_end = widx + w->num; widx < widx_end; widx++) {
	  if (*widx > max) break;
	  j = kwrk->tbl[*widx];
	  if (j != -1) {
	    qq += v->val[j];
	  }
	}
	*q = qq;
      }
    } else {	/* target=real-valued vector */
      for (q_end = q + kwrk->num, sv = kwrk->sv; q < q_end; q++, sv++) {
	float qq;

	w = *sv;
	if (w->num == 0) {
	  *q = 0.0f;
	  continue;
	}
	qq = 0.0f;
	for (widx = w->idx, widx_end = widx + w->num; widx < widx_end; widx++) {
	  if (*widx > max) break;
	  j = kwrk->tbl[*widx];
	  if (j != -1) {
	    qq += v->val[j] * w->val[widx - w->idx];
	  }
	}
	*q = qq;
      }
    }
  }

  /* 索引表の後片付け */
  for (vidx = v->idx, vidx_end = vidx + v->num; vidx < vidx_end; vidx++) {
    if (*vidx >= kwrk->max) break;
    kwrk->tbl[*vidx] = -1;
  }

  return;
}
