/*!
 * @file libharkio3_position.c
 *
 * @author Takeshi Mizumoto
 * @version 0.1
 * @brief  harkio_Position の API 群。 1つの対象 (マイク、音源など) の位置を表現する。
 */
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "libharkio3.h"

static const double pi = 3.14159265358979323846;
#define two_pi (2.0f * pi)

/*----------------------------------------------------------------------------*/
/* Generation                                                                 */
/*----------------------------------------------------------------------------*/

/*----------------------------------------------------------*/
/*                                                          */
/* coord[]: Polar     is dgree                              */
/*          Cartesian is metre                              */
/*----------------------------------------------------------*/
/*!
 * @brief コンストラクタ。空の 与えられた座標の POS を返す。
 * @return harkio_Position 型のポインタ。失敗すると NULL を返す。
 * @param[in]   id      harkio_Position の ID。
 * @param[in]   sys     harkio_Position の 座標系。
 * @param[in]   coord   harkio_Position の 座標そのもの。
 * @param[in]   path    harkio_Position に対応するファイルへのパス。wav　ファイルへのパスなど。
 * @details 使用が終わったら、 harkio_TransferFunction_delete でメモリ解放を行う。
 */
harkio_Position* harkio_Position_new(int id, enum Coordinate sys, float *coord, char *path) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int i;

    harkio_Position *p = (harkio_Position *) malloc(sizeof(harkio_Position));

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

    p->id = id;
    p->sys = sys;

    p->coord = (float *) malloc(sizeof(float) * 3);
    if (p->coord == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        return NULL;
    }

    for (i = 0; i < 3; i++) {
        p->coord[i] = coord[i];
    }

    if (path == NULL) {
        p->path = NULL;
    } else {
        p->path = (char *) malloc(sizeof(char) * (strlen(path) + 1));
        if (p->path == NULL) {
            harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
            free(p->coord);
            return NULL;
        }
        strcpy(p->path, path);
    }

    p->channelsUse = NON_TAG;
    p->numChannels = 0;
    p->channels = NULL;

    return p;
}

/*!
 * @brief デストラクタ。メンバになっている構造体を含めて全てのメモリを開放する。
 * @param[in] p  　　　開放したい伝達関数。解放後は NULL が代入される。
 */
void harkio_Position_delete(harkio_Position **p) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (p == NULL || *p == NULL) {
        return;
    }

    if ((*p)->coord) {
        free((*p)->coord);
    }

    if ((*p)->path) {
        free((*p)->path);
    }

    if ((*p)->channels) {
        free((*p)->channels);
    }

    free((*p));
    *p = NULL;
}

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

/*!
 * @brief Getter。harkio_Position の座標を返す。
 * @param[in] pos    harkio_Position 構造体へのポインタ
 * @return 座標が保持された float 配列へのポインタ
 */
float* harkio_Position_getPosition(harkio_Position* pos) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

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

    return pos->coord;
}

/*!
 * @brief Getter。harkio_Position の ID を返す。
 * @param[in] pos    harkio_Position 構造体へのポインタ
 * @return harkio_Position に対応する ID. if pos == NULL, return -1.
 */
int harkio_Position_getID(harkio_Position* pos) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (pos == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Position is NULL");
        return (-1);
    }

    return pos->id;
}

/*!
 * @brief Getter。harkio_Position の 座標系を返す。
 * @param[in] pos    harkio_Position 構造体へのポインタ
 * @return harkio_Position の座標系. if pos == NULL, return -1
 */
int harkio_Position_getSys(harkio_Position *pos) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (pos == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Position is NULL");
        return (-1);
    }

    return pos->sys;
}

/*!
 * @brief Setter。harkio_Position の 座標を更新する。
 * @param[in,out] pos    harkio_Position 構造体へのポインタ
 * @param[in]     sys    座標系
 * @param[in]     coord  座標
 * @return harkio_Position の座標系
 */
int harkio_Position_setPosition(harkio_Position* pos, enum Coordinate sys, float *coord) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

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

    pos->sys = sys;
    pos->coord[0] = coord[0];
    pos->coord[1] = coord[1];
    pos->coord[2] = coord[2];

    return EXIT_SUCCESS;

}

/*!
 * @brief Setter。harkio_Position の チャンネルを更新する。
 * @param[in,out] pos    harkio_Position 構造体へのポインタ
 * @param[in]     channels  チャンネルの配列. Copied. Accept NULL
 * @param[in]     numChannels  length of channels.
 * @return 成功すれば EXIT_SUCCSS、失敗すれば EXIT_FAILURE を返す
 * @details channels に合わせて、 numChannels の設定も必要。
 * @see harkio_Position_setNumChannels
 * @see harkio_Position_setChannelsUse
 */
int harkio_Position_setChannels(harkio_Position* pos, const int *channels, const int numChannels) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

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

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    if (pos->channels != NULL) {
        free(pos->channels);
    }

    pos->numChannels = numChannels;
    if (channels == NULL) {
        pos->channels = NULL;
    } else {
        pos->channels = (int *) malloc(sizeof(int) * numChannels);
        if (pos->channels == NULL)
            return EXIT_FAILURE;

        for (int i = 0; i < numChannels; i++) {
            pos->channels[i] = channels[i];
        }
    }
    return (EXIT_SUCCESS);
}

/*!
 * @brief Setter。harkio_Position の チャンネル数を更新する。
 * @param[in,out] pos    harkio_Position 構造体へのポインタ
 * @param[in]     numChannels  チャンネル数
 * @return 成功すれば EXIT_SUCCSS、失敗すれば EXIT_FAILURE を返す
 * @details channels に合わせて、 numChannels の設定も必要。
 * @see harkio_Position_setChannels
 * @see harkio_Position_setChannelsUse
 */
int harkio_Position_setNumChannels(harkio_Position* pos, int numChannels) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

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

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//

    pos->numChannels = numChannels;

    return (EXIT_SUCCESS);
}

/*!
 * @brief Setter。harkio_Position の チャンネル使用の有無を更新する。
 * @param[in,out] pos    harkio_Position 構造体へのポインタ
 * @param[in]     channels_use channels 使用の有無。 0なら不使用、1なら使用。
 * @return 成功すれば EXIT_SUCCSS、失敗すれば EXIT_FAILURE を返す
 * @details channels, numChannels の設定も必要。
 * @see harkio_Position_setNumChannels
 * @see harkio_Position_setChannels
 */
int harkio_Position_setChannelsUse(harkio_Position* pos, int channels_use) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

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

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//

    pos->channelsUse = channels_use;

    return (EXIT_SUCCESS);
}

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

/*!
 * @brief　harkio_Position をテキストで表示する
 * @param[in] pos     テキストで表示したい harkio_Position へのポインタ
 */
void harkio_Position_print(harkio_Position *pos) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    char *buffer = harkio_Position_printBuffer(pos);
    if (buffer)
        fprintf(stderr, "%s", buffer);
    free(buffer);
}

char* harkio_Position_printBuffer(harkio_Position *pos) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    char *buffer = (char*) malloc(MSG_LEN * sizeof(char));
    char tmp[4192];

    int i, len = 0;

    if (pos == NULL) {
        harkio_Log_printf(harkio_Log_Error, "pos is NULL");
        free(buffer);
        return NULL;
    }

    sprintf(buffer, "Position ->"
            "id: %d, "
            "sys: %d, "
            "path: %s, "
            "\n", pos->id, pos->sys, pos->path);

    if (pos->sys == Cartesian)
        len = 3;

    for (i = 0; i < len; i++) {
        sprintf(tmp, "   coord[%d]: %f, ", i, pos->coord[i]);
        strcat(buffer, tmp);
    }
    sprintf(tmp, "\n");
    strcat(buffer, tmp);

    for (i = 0; i < pos->numChannels; i++) {
        sprintf(tmp, "   channels: %d, ", pos->channels[i]);
        strcat(buffer, tmp);
    }
    sprintf(tmp, "\n");
    strcat(buffer, tmp);

    return buffer;
}

/*!
 * @brief　2つの harkio_Position が等しいか否かを判定する
 * @param[in] a   比較したい harkio_Position へのポインタ
 * @param[in] b   比較したい harkio_Position へのポインタ
 * @return 等しければ 1, 等しくなければ 0 を返す
 * @details ID、座標系、パスが等しく、すべての座標の値の差が HARK_EPS 以内であれば等しいと判断する。
 */
int harkio_Position_isEqual(harkio_Position *a, harkio_Position *b) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //--------------------------------------------//
    // Argument Check                             //
    //--------------------------------------------//
    if (a == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Position is NULL");
        return 0;
    }

    if (b == NULL) {
        harkio_Log_printf(harkio_Log_Error, "harkio_Position is NULL");
        return 0;
    }

    if (a->id != b->id) {
        harkio_Log_printf(harkio_Log_Debug, "id mismatch");
        return 0;
    }

    if (a->sys != b->sys) {
        harkio_Log_printf(harkio_Log_Debug, "sys mismatch");
        return 0;
    }

    if ((a->path != NULL && b->path != NULL) && strcmp(a->path, b->path) != 0) {
        harkio_Log_printf(harkio_Log_Debug, "path mismatch");
        return 0;
    }

    if ((a->path != NULL && b->path == NULL) || (a->path == NULL && b->path != NULL)) {
        harkio_Log_printf(harkio_Log_Debug, "path mismatch");
        return 0;
    }

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    if (a->sys == Cartesian) {
        int i;
        for (i = 0; i < 3; i++) {
            if (fabsf(a->coord[i] - b->coord[i]) > HARK_EPS) {
                return 0;
            }
        }
    } else if (a->sys == Polar) {
        float a_coord[3];
        float b_coord[3];
        //極座標を直交座標 に変換する
        harkio_Position_Convert(a, a_coord, Cartesian);
        harkio_Position_Convert(b, b_coord, Cartesian);

        if (fabsf(a_coord[0] - b_coord[0]) > HARK_EPS) {
            return 0;
        }

        if (fabsf(a_coord[1] - b_coord[1]) > HARK_EPS) {
            return 0;
        }

        if (fabsf(a_coord[2] - b_coord[2]) > HARK_EPS) {
            return 0;
        }
    }

    return 1;
}

/*!
 * @brief　harkio_Positions の座標を指定した座標系で得る。
 * @param[in]  pos      harkio_Position へのポインタ
 * @param[out] coord    変換された座標系
 * @param[in]  type     変換先の座標系
 * @return 成功すれば EXIT_SUCESS, 失敗すれば EXIT_FAILURE を返す。
 */
int harkio_Position_Convert(harkio_Position *pos, float *coord, enum Coordinate type) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    //C言語(C99)
    //cosf 関数は x の余弦 (cos) を計算し，結果を float 型で返します．
    //cosl 関数は x の余弦 (cos) を計算し，結果を long double 型で返します．
    //sinf 関数は x の正弦 (sin) を計算し，結果を float 型で返します．
    //sinl 関数は x の正弦 (sin) を計算し，結果を long double 型で返します．
    float azimuth;
    float elevation;
    float radius;
    float azimuth_radian;
    float elevation_radian;
    // float radius_radian;
    float x, y, z;
    float coef;

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

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//

    switch (type) {
    case Cartesian:

        azimuth = pos->coord[0];
        elevation = pos->coord[1];
        radius = pos->coord[2];

        //極座標を直交座標 に変換する
        azimuth_radian = harkio_Position_convToRadian(azimuth);     // azimuth
        elevation_radian = harkio_Position_convToRadian(elevation);   // elevation

        coef = radius * cosf(elevation_radian);

        coord[0] = coef * cosf(azimuth_radian);  // x
        coord[1] = coef * sinf(azimuth_radian);  // y
        coord[2] = radius * sinf(elevation_radian);  // z
        break;

    case Polar:
        //直交座標(x,y,z)を極座標 に変換する
        //θ = atan(y / x)
        //φ = atan(z / sqrt(x^2 + x^2))
        //r = sqrt(x^2 + y^2 + z^2)

        x = (pos->coord[0] == -0.0) ? 0.0 : pos->coord[0];
        y = (pos->coord[1] == -0.0) ? 0.0 : pos->coord[1];
        z = pos->coord[2];

        azimuth = atan2f(y, x);                      // azimuth
        elevation = atan2f(z, sqrtf(x * x + y * y)); // elevation
        radius = sqrtf((x * x) + (y * y) + (z * z)); // radius

        coord[0] = harkio_Position_convToDegree(azimuth);
        coord[1] = harkio_Position_convToDegree(elevation);
        coord[2] = radius;
        break;

    default:
        harkio_Log_printf(harkio_Log_Error, "type is Illegal value");
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;

}

/*!
 * @brief　harkio_Position をコピーする
 * @param[in] p         コピーしたい harkio_Position へのポインタ
 * @return コピーに成功すれば コピーした harkio_Position へのポインタ, 失敗すれば NULL を返す。
 */
harkio_Position* harkio_Position_copy(harkio_Position *p) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    int *channels = NULL;
    float *coord = NULL;
    int i;

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

    //--------------------------------------------//
    // main                                       //
    //--------------------------------------------//
    char *path = NULL;
    if (p->path) {
        path = (char *) malloc(sizeof(char) * (strlen(p->path) + 1));
        if (path == NULL) {
            harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
            return NULL;
        }
        strcpy(path, p->path);
    }

    coord = (float *) malloc(sizeof(float) * 3);
    if (coord == NULL) {
        harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
        if (path)
            free(path);
        return NULL;
    }

    for (i = 0; i < 3; i++) {
        coord[i] = p->coord[i];
    }

    if (p->numChannels != 0) {
        channels = (int*) malloc(sizeof(int) * p->numChannels);
        if (channels == NULL) {
            harkio_Log_printf(harkio_Log_Error, "can not allocated memory");
            free(path);
            free(coord);
            return NULL;
        }

        for (i = 0; i < p->numChannels; i++) {
            channels[i] = p->channels[i];
        }
    }

    harkio_Position *position = harkio_Position_new(p->id, p->sys, coord, path);

    harkio_Position_setNumChannels(position, p->numChannels);
    harkio_Position_setChannels(position, channels, p->numChannels);
    harkio_Position_setChannelsUse(position, p->channelsUse);

    if (path)
        free(path);
    free(coord);

    if (channels != NULL) {
        free(channels);
    }

    return position;
}

/*!
 * @brief　内部用関数。 角度(degree)からラジアン(radian)に変換
 */
float harkio_Position_convToRadian(float degree) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    return degree * pi / 180.0f;
}

/*!
 * @brief　内部用関数。 ラジアン(radian)から角度(degree)に変換
 */
float harkio_Position_convToDegree(float radian) {
    harkio_Log_printf(harkio_Log_Trace, "@ %s", __func__);

    return radian * 180.0f / pi;

}
