/*
   HARK Library
   Copyright (C) 2021 Honda Research Institute Japan Inc.
 */

#include "hark-lib/source.h"

#include <math.h>
#include <chrono>

namespace hark
{
    Source::Source()
    {
        x[0] = 0.0f;
        x[1] = 0.0f;
        x[2] = 0.0f;
        power = 0.0f;
        id = 0;
        sub_id = 0;
        mic_id = 0;
        remaining = 0.0f;
        count = 0;
        last_found_count = 0;
        tfindex[0] = 0;
        tfindex[1] = 0;
        tfindex[2] = 0;
        tfindex_min[0] = 0;
        tfindex_min[1] = 0;
        tfindex_min[2] = 0;
        tfindex_max[0] = 0;
        tfindex_max[1] = 0;
        tfindex_max[2] = 0;
        compare_mode = Source::TFINDEX;
    }

    Source::Source(const Source &src)
    {
        x[0] = src.x[0];
        x[1] = src.x[1];
        x[2] = src.x[2];
        power = src.power;
        id = src.id;
        sub_id = src.sub_id;
        mic_id = src.mic_id;
        remaining = src.remaining;
        count = src.count;
        last_found_count = src.last_found_count;

        // inserted for index search
        for(int i = 0; i < 3; i++){
            tfindex[i] = src.tfindex[i];
            tfindex_min[i] = src.tfindex_min[i];
            tfindex_max[i] = src.tfindex_max[i];
        }
        compare_mode = src.compare_mode;
    }

    int
    Source::tfindexDistance(const int *y) const
    {
        int maxdist = 0;
        for(int i = 0; i < 3; i++){
            int tmpdist;
            if(tfindex[i] > y[i]){
                tmpdist = std::min(std::abs(tfindex[i] - y[i]),
                                   std::abs(tfindex[i] - tfindex_max[i]) + std::abs(y[i] - tfindex_min[i]));
            }else{
                tmpdist = std::min(std::abs(tfindex[i] - y[i]),
                                   std::abs(tfindex[i] - tfindex_min[i]) + std::abs(y[i] - tfindex_max[i]));
            }
            maxdist = (maxdist < tmpdist)? tmpdist: maxdist;
        }
        return maxdist;
    }

    int
    Source::tfindexDistance(const Source &src) const
    {
        int maxdist = 0;
        for(int i = 0; i < 3; i++){
            int tmpdist;
            if(tfindex[i] > src.tfindex[i]){
                tmpdist = std::min(abs(tfindex[i] - src.tfindex[i]),
                                   std::abs(tfindex[i] - tfindex_max[i]) + std::abs(src.tfindex[i] - tfindex_min[i]));
            }else{
                tmpdist = std::min(abs(tfindex[i] - src.tfindex[i]),
                                   std::abs(tfindex[i] - tfindex_min[i]) + std::abs(src.tfindex[i] - tfindex_max[i]));
            }
            maxdist = (maxdist < tmpdist)? tmpdist: maxdist;
        }
        return maxdist;
    }

    float
    Source::angle(const float *y) const
    {
        float prod = (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]) / sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]) / sqrtf(y[0] * y[0] + y[1] * y[1] + y[2] * y[2]);
        if(prod < -1){
            prod = -1;
        }
        if(prod > 1){
            prod = 1;
        }
        return acosf(prod);
    }

    float
    Source::angle(const Source &src) const
    {
        float prod = (x[0] * src.x[0] + x[1] * src.x[1] + x[2] * src.x[2])
                     / sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2])
                     / sqrtf(src.x[0] * src.x[0] + src.x[1] * src.x[1] + src.x[2] * src.x[2]);
        return acosf(prod);
    }

    std::ostream &
    Source::write(std::ostream &os) const
    {
        os << "{"
           << "id: " << this->id
           << ", sub_id: " << this->sub_id
           << ", mic_id: " << this->mic_id
           << ", x: [" << this->x[0] << ", " << this->x[1] << ", " << this->x[2] << "]"
           << ", power: " << this->power
           << ", remaining: " << this->remaining
           << ", count: " << this->count
           << ", first_found_count: " << this->first_found_count
           << ", last_found_count: " << this->last_found_count
           << ", tfindex: [" << this->tfindex[0] << ", " << this->tfindex[1] << ", " << this->tfindex[2] << "]"
           << ", tfindex_max: [" << this->tfindex_max[0] << ", " << this->tfindex_max[1] << ", " << this->tfindex[2] << "]"
           << ", tfindex_min: [" << this->tfindex_min[0] << ", " << this->tfindex_min[1] << ", " << this->tfindex[2] << "]"
           << ", compare_mode: " << ((this->compare_mode == Source::CompareMode::DEG) ? "DEG" : "TFINDEX")
           << "}";
        return os;
    }

    UtteranceInfo::UtteranceInfo():
        client_id(0), mic_index(0), source_id(0), sub_id(0), azimuth(0), elevation(0), tv_sec(0), tv_usec(0)
    {
        auto now = std::chrono::system_clock::now();
        //gettimeofday(&this->time, nullptr);
        this->tv_sec = std::chrono::system_clock::to_time_t(now);
        auto usecs = std::chrono::time_point_cast<std::chrono::microseconds>(now);
        this->tv_usec = usecs.time_since_epoch().count() % 1000000;
    }

    UtteranceInfo::UtteranceInfo(const UtteranceInfo &src):
        client_id(src.client_id), mic_index(src.mic_index), source_id(src.source_id), sub_id(src.sub_id), azimuth(src.azimuth), elevation(src.elevation), tv_sec(src.tv_sec), tv_usec(src.tv_usec)
    {
    }

    std::ostream &
    UtteranceInfo::write(std::ostream &os) const
    {
        os << "{"
           << "client_id: " << this->client_id
           << ", mic_index: " << this->mic_index
           << ", source_id: " << this->source_id
           << ", sub_id: " << this->sub_id
           << ", azimuth: " << this->azimuth
           << ", elevation: " << this->elevation
           << ", tv_sec: " << this->tv_sec
           << ", tv_usec: " << this->tv_usec
           << "}";
        return os;
    }

// ObjectRef
// Source::clone()
// {
//     return ObjectRef(new Source(*this));
// }
//
// void
// Source::printOn(std::ostream& out) const
// {
//     out << "<Source " << std::endl;
//     out << "<x " << x[0] << " " << x[1] << " " << x[2] << " >" << std::endl;
//     out << "<power " << power << " >" << std::endl;
//     out << "<id " << id << " >" << std::endl;
//     out << ">" << std::endl;
// }
//
// void
// Source::readFrom(std::istream &in)
// {
// }
//
// std::istream &
// operator >> (std::istream &in, Source &src)
// {
//     if (!isValidType(in, "Source")) {
//         return in;
//     }
//     src.readFrom(in);
//     return in;
// }
}

/* end of file */
