/*!
 * @file libharkio3_neighbors.c
 *
 * @author Takeshi Mizumoto
 * @version 0.1
 * @brief  harkio_Neighbor の API 群。 近接 Position の計算・取り出し・表現などを行う。
 */

#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include <float.h>

#include "libharkio3.h"

/*----------------------------------------------------------------------------*/
/* Generation                                                                 */
/*----------------------------------------------------------------------------*/
/*!
 * @brief コンストラクタ。近接関係を表すオブジェクトを生成する。
 * @return harkio_Neighbors 型のポインタ。失敗すると NULL を返す
 * @param[in]   poses  近接関係を表現したい位置群を与える。 NULL でもよい。
 * @details poses はコピーせずにポインタが渡される。
 *         NULL を与えた場合は位置が無い空のオブジェクトが生成される。
 *         使用が終わったら、 harkio_Neighbors_delete でメモリ解放を行う。
 */
harkio_Neighbors* harkio_Neighbors_new(harkio_Positions *poses) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i;

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    harkio_Neighbors *nbrs = (harkio_Neighbors *) malloc(sizeof(harkio_Neighbors));

    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return NULL;
    }

    if (poses == NULL) {
        nbrs->size = 0;
        nbrs->ids = NULL;
        nbrs->numNeighbors = NULL;
        nbrs->neighbors = NULL;
        nbrs->poses = NULL;
        nbrs->alg = Undefined;
        return nbrs;
    }

    // setup members
    nbrs->size = poses->size;

    // memory alloc
    nbrs->ids = (int *) malloc(sizeof(int) * nbrs->size);
    if (nbrs->ids == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        free(nbrs);
        return NULL;
    }

    nbrs->numNeighbors = (int *) malloc(sizeof(int) * nbrs->size);
    if (nbrs->numNeighbors == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        free(nbrs->ids);
        free(nbrs);
        return NULL;
    }

    nbrs->neighbors = (int **) calloc(sizeof(int*), nbrs->size);
    if (nbrs->neighbors == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        free(nbrs->ids);
        free(nbrs->numNeighbors);
        free(nbrs);
        return NULL;
    }

    for (i = 0; i < nbrs->size; i++) {
        nbrs->ids[i] = poses->pos[i]->id;
        nbrs->numNeighbors[i] = 0;
    }

    nbrs->poses = poses;
    nbrs->alg = Undefined;

    return nbrs;
}

/*!
 * @brief デストラクタ。harkio_Neighbors のメモリを解放する。
 * @param[in] nbrs   メモリを解放するオブジェクトへのポインタへのポインタ
 * @details 解放に成功すると xml には NULL が代入される。メンバのいくつかが NULL でもよい。
 */
void harkio_Neighbors_delete(harkio_Neighbors **nbrs) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL || *nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "nbrs is NULL");
        return;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    int i;
    for (i = 0; i < (*nbrs)->size; i++) {
        if ((*nbrs)->neighbors[i])
            free((*nbrs)->neighbors[i]);
    }

    free((*nbrs)->ids);
    free((*nbrs)->numNeighbors);
    free((*nbrs)->neighbors);
    free((*nbrs));
    *nbrs = NULL;
}

/*----------------------------------------------------------------------------*/
/* accessor                                                                   */
/*----------------------------------------------------------------------------*/

/*!
 * @brief getter。与えられた id の近接 id を返す。[]
 * @param[in]  nbrs  近接 ID を検索したい hakrio_Neighbors オブジェクト
 * @param[in]  id    近接 ID を検索したい対象の ID
 * @param[out] numNeighbors  見つかった近接 ID の個数
 * @param[out] neighbors     見つかった近接 ID の配列へのポインタ
 * @details 引数に不備があれば EXIT_FAILURE が返される。成功すれば numNeighbors, neighbors　に値がセットされ
 *         EXIT_SUCCESS が返る。 neighbors は関数内で malloc されるので free が必要。
 * @code
 int numNeighbors;
 int *neighbors;
 ret = harkio_Neighbors_getNeighbors(nbrs, targetID, &numNeighbors, &neighbors);
 * @endcode
 */
int harkio_Neighbors_getNeighbors(harkio_Neighbors *nbrs, int id, int *numNeighbors, int **neighbors) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "nbrs is NULL");
        return EXIT_FAILURE;
    }

    if (nbrs->alg == Undefined) {
        harkio_Log_printf(harkio_Log_Error, "alg is Undefined");
        return EXIT_FAILURE;
    }

    //find the id
    for (i = 0; i < nbrs->size; i++) {
        if (nbrs->ids[i] == id) {
            break;
        }
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    // if not found, return EXIT_FAILURE
    if (i == nbrs->size) {
        numNeighbors = NULL;
        neighbors = NULL;

        harkio_Log_printf(harkio_Log_Error, "neighbors is not found.");
        return EXIT_FAILURE;
    }

    *numNeighbors = nbrs->numNeighbors[i];
    *neighbors = nbrs->neighbors[i];
    fprintf(stderr, "numNeighbors: %d, neighrbos: %p\n", *numNeighbors, *neighbors);
    // fprintf(stderr, "nbrs[0]=%d, nbrs[1]=%d\n", nbrs->neighbors[i][0], nbrs->neighbors[i][1]);
    // fprintf(stderr, "nbrs[0]=%d, nbrs[1]=%d\n", neighbors[0], neighbors[1]);
    return EXIT_SUCCESS;
}

/*!
 * @brief Getter. harkio_Neighbors に登録されている harkio_Positions の長さ
 * @param[in] nbs harkio_Neighbors オブジェクト
 */
int harkio_Neighbors_getSize(harkio_Neighbors *nbs) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Neighbors is NULL");
        return EXIT_FAILURE;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    return nbs->size;
}

/*!
 * @brief Getter. harkio_Neighbors の近接関係計算アルゴリズムを返す。
 * @param[in] nbs harkio_Neighbors オブジェクト
 */
enum NeighborAlgorithm harkio_Neighbors_getNeighborAlgorithm(harkio_Neighbors *nbs) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Neighbors is NULL");
        return EXIT_FAILURE;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    return nbs->alg;
}

/*----------------------------------------------------------------------------*/
/* Operation                                                                  */
/*----------------------------------------------------------------------------*/

/*!
 * @brief harkio_Neighbors に登録されている位置の近接関係を計算する。
 * @param[in] nbs harkio_Neighbors オブジェクト
 * @param[in] alg 近接関係計算アルゴリズムを表す enum.
 * @param[in] ... アルゴリズムごとに決まる可変引数。
 * @return 計算に成功すれば EXIT_SUCCESS, 失敗すれば EXIT_FAILURE を返す。
 */
int harkio_Neighbors_calcNeighbors(harkio_Neighbors *nbrs, enum NeighborAlgorithm alg, ...) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int ret;
    int n;
    float thresh; // for NearesteNeighbor
    va_list args;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Neighbors is NULL");
        return EXIT_FAILURE;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    switch (alg) {
    case NearestNeighbor:
        va_start(args, alg);
        n = va_arg(args, int);
        thresh = (float) va_arg(args, double);
        va_end(args);

        nbrs->alg = alg;
        ret = harkio_Neighbors_Algorithms_NearestNeighbor(nbrs, n, thresh);
        break;

    default:
        harkio_Log_printf(harkio_Log_Error, "alg is Unknown.");
        ret = EXIT_FAILURE;
    }

    return ret;
}

/*!
 * @brief 新しい隣接関係を追加する。与えられた ID に隣接関係がすでに定義されていれば追加し、定義が無ければ新しく作成する。
 * @param[in] nbs harkio_Neighbors オブジェクト
 * @param[in] id  追加する id
 * @param[in] numNeighbors 追加する隣接点の数
 * @param[in] numNeighbors 追加する隣接へのポインタ
 * @return 成功すれば EXIT_SUCCESS, 失敗すれば EXIT_FAILURE.
 */
int harkio_Neighbors_append(harkio_Neighbors *nbrs, int id, int numNeighbors, int *neighbors) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int isIdAlreadyExists = 0;
    int foundIndex = -1;
    int i;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Neighbors is NULL");
        return EXIT_FAILURE;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    // decide whether id is already exist or not.
    for (i = 0; i < nbrs->size; i++) {
        if (id == nbrs->ids[i]) {
            isIdAlreadyExists = 1;
            foundIndex = i;
        }
    }

    //already exists
    if (isIdAlreadyExists == 1) {
        int *dummy;
        int counter = 0;
        nbrs->numNeighbors[foundIndex] += numNeighbors;

        // dummy = (int *) malloc(sizeof(int) * nbrs->numNeighbors[foundIndex])
        dummy = (int *) realloc(nbrs->neighbors[foundIndex], sizeof(int) * nbrs->numNeighbors[foundIndex]);
        if (dummy) {
            nbrs->neighbors[foundIndex] = dummy;
        } else { //realloc fail
            //free(dummy);
            harkio_Log_printf(harkio_Log_Error, "realloc is Failed");
            return EXIT_FAILURE;
        }

        for (i = nbrs->numNeighbors[foundIndex] - numNeighbors; i < nbrs->numNeighbors[foundIndex]; i++) {
            nbrs->neighbors[foundIndex][i] = neighbors[counter++];
        }
    }
    //Not exist
    else {
        int newLastIndex = nbrs->size;
        int *dummy;   // for realloc
        int **dummy2; // for realloc
        nbrs->size++;

        // reallocate memories for nbrs->ids, nbrs->numNeighbors, and nbrs->neigbors;
        dummy = (int *) realloc(nbrs->ids, sizeof(int) * nbrs->size);

        if (dummy) {
            nbrs->ids = dummy;
        } else {
            harkio_Log_printf(harkio_Log_Error, "realloc is Failed");
            return EXIT_FAILURE;
        }

        dummy = (int *) realloc(nbrs->numNeighbors, sizeof(int) * nbrs->size);

        if (dummy) {
            nbrs->numNeighbors = dummy;
        } else {
            harkio_Log_printf(harkio_Log_Error, "realloc is Failed");
            return EXIT_FAILURE;
        }

        dummy2 = (int **) realloc(nbrs->neighbors, sizeof(int*) * nbrs->size);

        if (dummy2) {
            nbrs->neighbors = dummy2;
        } else {
            harkio_Log_printf(harkio_Log_Error, "realloc is Failed");
            return EXIT_FAILURE;
        }

        // copy data
        nbrs->ids[newLastIndex] = id;
        nbrs->numNeighbors[newLastIndex] = numNeighbors;
        nbrs->neighbors[newLastIndex] = (int *) malloc(sizeof(int) * nbrs->numNeighbors[newLastIndex]);

        if (nbrs->neighbors[newLastIndex] == NULL) {
            harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
            return EXIT_FAILURE;
        }

        for (i = 0; i < nbrs->numNeighbors[newLastIndex]; i++) {
            nbrs->neighbors[newLastIndex][i] = neighbors[i];
        }
    }

    return EXIT_SUCCESS;
}

/*----------------------------------------------------------------------------*/
/* Utility                                                                  */
/*----------------------------------------------------------------------------*/

/*!
 * @brief 近接関係計算アルゴリズムの実装。近傍をクラスタリングする方法。
 * @param[in] nbrs harkio_Neighbors オブジェクト
 * @param[in] n   近接関係と見なす最大数
 * @param[in] thresh 近接か否かを判定するしきい値
 * @return 成功すれば EXIT_SUCCESS, 失敗すれば EXIT_FAILURE.
 * @details nbrs が持つ位置のユークリッド距離をすべてについて計算し、
 *         thresh より小さいものを近傍として登録する。ただし、その数が n より大きければ近い順に n 個だけ登録する。
 */
int harkio_Neighbors_Algorithms_NearestNeighbor(harkio_Neighbors *nbrs, int n, float thresh) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i, j, k, neighborCounter;
    int *closestIDs;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Neighbors is NULL");
        return EXIT_FAILURE;
    }

    if (n <= 0) {
        harkio_Log_printf(harkio_Log_Error, "n-nearest neighrbos must be (n > 0)");
        return EXIT_FAILURE;    // n-nearest neighrbos must be (n > 0)
    }

    if (thresh < 0) {
        harkio_Log_printf(harkio_Log_Error, "threshold must be positive");
        return EXIT_FAILURE;    // threshold must be positive
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    closestIDs = (int*) malloc(sizeof(int) * n);

    if (closestIDs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return EXIT_FAILURE;
    }

    // main algorithm
    // for each nbrs->poses->pos, find n-closest pos.
    for (i = 0; i < nbrs->size; i++) {
        // reset closest ids

        // find the (neighborCounter)-th clothest id from i-th position
        for (neighborCounter = 0; neighborCounter < n; neighborCounter++) {
            float min = FLT_MAX;
            int minid = -999;

            for (j = 0; j < nbrs->size; j++) {
                float tmpdist = euclidDistance(nbrs->poses->pos[i], nbrs->poses->pos[j], 3);

                int isAlreadyFound = 0;

                // if pos[j]->id is already in closestIDs, skip it.
                for (k = 0; k < neighborCounter; k++)
                    if (nbrs->poses->pos[j]->id == closestIDs[k])
                        isAlreadyFound = 1;
                if (isAlreadyFound)
                    continue;

                if (min > tmpdist) {
                    min = tmpdist;
                    minid = nbrs->poses->pos[j]->id;
                }
            }
            // if the smallest distance is larger than thresh, finish searching.
            if (min > thresh)
                break;
            closestIDs[neighborCounter] = minid;
        }

        nbrs->numNeighbors[i] = neighborCounter;
        nbrs->neighbors[i] = (int *) malloc(sizeof(int) * neighborCounter);

        if (nbrs->neighbors[i] == NULL) {
            harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
            free(closestIDs);
            return EXIT_FAILURE;
        }

        for (j = 0; j < neighborCounter; j++)
            nbrs->neighbors[i][j] = closestIDs[j];
    }
    free(closestIDs);
    return EXIT_SUCCESS;
}

/*!
 * @brief 内部用関数。二つの harkio_Position の距離を求める
 */
float euclidDistance(harkio_Position *pos_a, harkio_Position *pos_b, int length) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    float sum = 0;
    int i;

    float a_coord[3];
    float b_coord[3];

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (pos_a == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return EXIT_FAILURE;
    }

    if (pos_b == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return EXIT_FAILURE;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    for (i = 0; i < length; i++) {
        //init
        a_coord[i] = 0;
        b_coord[i] = 0;

        if (pos_a->sys == Polar) {
            harkio_Position_Convert(pos_a, a_coord, Cartesian);
        } else {
            a_coord[i] = pos_a->coord[i];
        }

        if (pos_b->sys == Polar) {
            harkio_Position_Convert(pos_b, b_coord, Cartesian);
        } else {
            b_coord[i] = pos_b->coord[i];
        }

        sum += pow(a_coord[i] - b_coord[i], 2);
    }

    sum = sqrt(sum);

    return sum;
}

/*!
 * @brief harkio_Neighbors の内容を stderr に表示する。
 */
void harkio_Neighbors_print(harkio_Neighbors *nbrs) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i, j;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (nbrs == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    fprintf(stderr, "Neighbors:\n");
    for (i = 0; i < nbrs->size; i++) {
        fprintf(stderr, "[ID: %d (numNeighbors: %d)]", nbrs->ids[i], nbrs->numNeighbors[i]);
        for (j = 0; j < nbrs->numNeighbors[i]; j++) {
            fprintf(stderr, " %d", nbrs->neighbors[i][j]);
        }
        fprintf(stderr, "\n");
    }
}

/*!
 * @brief harkio_Neighbors が等しいかどうかを比較する。
 * @return 等しければ EXIT_SUCCESS, 異なれば EXIT_FAILURE
 */
int harkio_Neighbors_isEqual(harkio_Neighbors *a, harkio_Neighbors *b) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i, j;
    harkio_Log_printf(harkio_Log_Debug, "@ harkio_Neighbors_isEqual");
    fprintf(stderr, "--start---\n");
    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (a == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return 0;
    }

    if (b == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return 0;
    }

    fprintf(stderr, "-----\n");

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    if (a->size != b->size) {
        harkio_Log_printf(harkio_Log_Error, "size mismatch");
        return 0;
    }

    fprintf(stderr, "-----\n");
    // compare neighbors
    for (i = 0; i < a->size; i++) {
        if (a->ids[i] != b->ids[i]) {
            return 0;
        }

        if (a->numNeighbors[i] != b->numNeighbors[i]) {
            return 0;
        }

        for (j = 0; j < a->numNeighbors[i]; j++) {
            if (a->neighbors[i][j] != b->neighbors[i][j]) {
                return 0;
            }
        }
    }

    fprintf(stderr, "-----\n");
    // compare pos
    if (harkio_Positions_isEqual(a->poses, b->poses) == 0) {
        fprintf(stderr, "-not equal positions----\n");
        harkio_Log_printf(harkio_Log_Error, "positions mismatch");
        return 0;
    }

    fprintf(stderr, "-done----\n");
    return 1;
}

/*!
 * @brief harkio_Neighbors の内容を コピーする
 * @return 与えられた harkio_Neighbors をコピーした新しいオブジェクト
 */
harkio_Neighbors* harkio_Neighbors_copy(harkio_Neighbors *n1) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i;

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (n1 == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return NULL;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    harkio_Neighbors *n2 = harkio_Neighbors_new(NULL);

    if (n2 == NULL) {
        harkio_Log_printf(harkio_Log_Error, "could not create new harkio_Neighbors.");
        return NULL;
    }

    for (i = 0; i < n1->size; i++) {
        harkio_Neighbors_append(n2, n1->ids[i], n1->numNeighbors[i], n1->neighbors[i]);
    }
    n2->alg = n1->alg;
    return n2;
}
