yatemath.h

#ifndef __YATEMATH_H
#define __YATEMATH_H

#include <yateclass.h>
#include <math.h>
#include <string.h>

namespace TelEngine {

#ifdef DEBUG
#ifdef _WINDOWS
#define YMATH_FAIL(cond,...) { \
    if (!(cond)) \
        Debug(DebugFail,__VA_ARGS__); \
}
#else
#define YMATH_FAIL(cond,args...) { \
    if (!(cond)) \
        Debug(DebugFail,args); \
}
#endif
#else
#ifdef _WINDOWS
#define YMATH_FAIL do { break; } while
#else
#define YMATH_FAIL(arg...)
#endif
#endif

class YATE_API Complex
{
public:
    inline Complex()
        : m_real(0), m_imag(0)
        {}

    inline Complex(float real, float imag = 0)
        : m_real(real), m_imag(imag)
        {}

    inline Complex(const Complex& c)
        : m_real(c.m_real), m_imag(c.m_imag)
        {}

    inline float re() const
        { return m_real; }

    inline void re(float r)
        { m_real = r; }

    inline float im() const
        { return m_imag; }

    inline void im(float i)
        { m_imag = i; }

    inline Complex& set(float r = 0, float i = 0) {
            m_real = r;
            m_imag = i;
            return *this;
        }

    inline bool operator==(const Complex& c) const
        { return m_real == c.m_real && m_imag == c.m_imag; }

    inline bool operator!=(const Complex& c) const
        { return m_real != c.m_real || m_imag != c.m_imag; }

    inline Complex& operator=(const Complex& c)
        { return set(c.m_real,c.m_imag); }

    inline Complex& operator=(float real)
        { return set(real); }

    inline Complex& operator+=(const Complex& c)
        { return set(m_real + c.m_real,m_imag + c.m_imag); }

    inline Complex& operator+=(float real) {
            m_real += real;
            return *this;
        }

    inline Complex& operator-=(const Complex& c)
        { return set(m_real - c.m_real,m_imag - c.m_imag); }

    inline Complex& operator-=(float real) {
            m_real -= real;
            return *this; 
        }

    inline Complex& operator*=(const Complex& c) {
            return set(m_real * c.m_real - m_imag * c.m_imag,
                m_real * c.m_imag + m_imag * c.m_real);
        }

    inline Complex& operator*=(float f)
        { return set(m_real * f,m_imag * f); }

    inline Complex& operator/=(const Complex& c) {
            float tmp = c.norm2();
            return set((m_real * c.m_real + m_imag * c.m_imag) / tmp,
                (-m_real * c.m_imag + m_imag * c.m_real) / tmp);
        }

    inline Complex& operator/=(float f)
        { return set(m_real / f,m_imag / f); }

    inline float abs() const
        { return ::sqrtf(norm2()); }

    inline float mod() const
        { return abs(); }

    inline float arg() const
        { return ::atan(m_imag / m_real); }

    inline Complex exp() const {
            float r = ::expf(m_real);
            return Complex(r * ::cosf(m_imag),r * ::sinf(m_imag));
        }

    inline float norm() const
        { return abs(); }

    inline float norm2() const
        { return m_real * m_real + m_imag * m_imag; }

private:
    float m_real;                        // The real part
    float m_imag;                        // The imaginary part
};


class YATE_API RefStorage : public RefObject
{
    YCLASS(RefStorage,RefObject)
    YNOCOPY(RefStorage); // No automatic copies please
public:
    inline RefStorage(const void* value, unsigned int len)
        : m_data((void*)value,len)
        {}

    inline unsigned int length() const
        { return m_data.length(); }

    inline void* data(unsigned int offs, unsigned int len) const
        { return len ? m_data.data(offs,len) : 0; }

    inline bool set(const void* buf, unsigned int len, unsigned int offs = 0)
        { return copy(data(offs,len),buf,len); }

    static inline void fill(void* dest, unsigned int len, int val = 0) {
            if (dest && len)
                ::memset(dest,val,len);
        }

    static inline bool copy(void* dest, const void* src, unsigned int len) {
            if (!(len && dest && src))
                return len == 0;
            if (dest != src)
                ::memcpy(dest,src,len);
            return true;
        }

    static inline bool equals(const void* buf1, const void* buf2, unsigned int len) {
            if (len && buf1 && buf2)
                return (buf1 == buf2) || (::memcmp(buf1,buf2,len) == 0);
            return true;
        }

    static String& dumpSplit(String& buf, const String& str, unsigned int lineLen,
        unsigned int offset = 0, const char* linePrefix = 0,
        const char* suffix = "\r\n");

private:
    RefStorage() {}; // No default constructor please

    DataBlock m_data;
};


class YATE_API MathVectorBase : public GenObject
{
    YCLASS(MathVectorBase,GenObject)
public:
    virtual ~MathVectorBase()
        {}

    virtual unsigned int vectorSize() const = 0;
};


template <class Obj> class SliceVector : public MathVectorBase
{
public:
    inline SliceVector()
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0)
        {}

    inline SliceVector(const SliceVector& other)
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0)
        { initSlice(false,other); }

    explicit inline SliceVector(unsigned int len, const Obj* buf = 0,
        unsigned int maxLen = 0)
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0)
        { initStorage(len,buf,maxLen); }

    explicit inline SliceVector(const SliceVector& v1, const SliceVector& v2)
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0) {
            if (!initStorage(v1.length(),v1.data(),v1.length() + v2.length()))
                return;
            resizeMax();
            m_storage->set(v2.data(),v2.size(),v1.size());
        }

    explicit inline SliceVector(const SliceVector& v1, const SliceVector& v2,
        const SliceVector& v3)
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0) {
            unsigned int n = v1.length() + v2.length() + v3.length();
            if (!initStorage(v1.length(),v1.data(),n))
                return;
            resizeMax();
            m_storage->set(v2.data(),v2.size(),v1.size());
            m_storage->set(v3.data(),v3.size(),v1.size() + v2.size());
        }

    explicit inline SliceVector(const SliceVector& other, unsigned int offs,
        unsigned int len = 0)
        : m_storage(0), m_data(0), m_length(0), m_maxLen(0)
        { initSlice(false,other,offs,len); }

    virtual ~SliceVector()
        { setData(); }

    inline Obj* data(unsigned int offs, unsigned int len) {
            if (len && length() && offs + len <= length())
                return m_data + offs;
            return 0;
        }

    inline const Obj* data(unsigned int offs, unsigned int len) const {
            if (len && length() && offs + len <= length())
                return m_data + offs;
            return 0;
        }

    inline Obj* data(unsigned int offs = 0)
        { return data(offs,available(offs)); }

    inline const Obj* data(unsigned int offs = 0) const
        { return data(offs,available(offs)); }

    inline Obj* data(unsigned int offs, unsigned int len, Obj*& eod) {
            Obj* d = data(offs,len);
            eod = end(d,len);
            return d;
        }

    inline const Obj* data(unsigned int offs, unsigned int len, const Obj*& eod) const {
            const Obj* d = data(offs,len);
            eod = end(d,len);
            return d;
        }

    inline unsigned int length() const
        { return m_length; }

    inline unsigned int maxLen() const
        { return m_maxLen; }

    inline unsigned int size() const
        { return size(length()); }

    inline unsigned int available(unsigned int offs, int len = -1) const {
            if (len && offs < length()) {
                unsigned int rest = length() - offs;
                return (len < 0 || rest <= (unsigned int)len) ? rest : (unsigned int)len;
            }
            return 0;
        }
        
    inline unsigned int availableClamp(unsigned int clamp, unsigned int offs = 0,
        int len = -1) const {
            offs = available(offs,len);
            return clamp <= offs ? clamp : offs;
        }

    virtual unsigned int vectorSize() const
        { return size(); }

    inline bool resize(unsigned int len) {
            if (len <= maxLen()) {
                m_length = len;
                return true;
            }
            return false;
        }

    inline void resizeMax()
        { resize(maxLen()); }

    inline void steal(SliceVector& other) {
            m_storage = other.m_storage;
            m_data = other.m_data;
            m_length = other.m_length;
            m_maxLen = other.m_maxLen;
            other.m_storage = 0;
            other.m_data = 0;
            other.m_length = other.m_maxLen = 0;
        }

    inline void resetStorage(unsigned int len, unsigned int maxLen = 0) {
            setData();
            initStorage(len,0,maxLen);
        }

    inline bool setSlice(const SliceVector& other, unsigned int offs = 0,
        unsigned int len = 0)
        { return initSlice(true,other,offs,len); }

    inline SliceVector head(unsigned int len) const
        { return slice(0,len); }

    inline bool head(SliceVector& dest, unsigned int len) const
        { return slice(dest,0,len); }

    inline SliceVector tail(unsigned int len) const {
            if (len < length())
                return SliceVector(*this,length() - len,len);
            return SliceVector();
        }

    inline bool tail(SliceVector& dest, unsigned int len) const {
            if (len <= length())
                return dest.initSlice(true,*this,length() - len,len);
            dest.setData();
            return false;
        }

    inline SliceVector slice(unsigned int offs, unsigned int len) const
        { return SliceVector(*this,offs,len); }

    inline bool slice(SliceVector& dest, unsigned int offs,
        unsigned int len = 0) const
        { return dest.initSlice(true,*this,offs,len); }

    inline bool copy(const SliceVector& src, unsigned int len,
        unsigned int offs = 0, unsigned int srcOffs = 0)
        { return RefStorage::copy(data(offs,len),src.data(srcOffs,len),size(len)); }

    inline void bzero(unsigned int offs, unsigned int len)
        { RefStorage::fill(data(offs,len),size(len)); }

    inline void bzero()
        { RefStorage::fill(data(),size()); }

    inline void fill(const Obj& value) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                *d = value;
        }

    inline void apply(void (*func)(Obj&)) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                (*func)(*d);
        }

    inline Obj sum() const {
            Obj result(0);
            const Obj* d = data();
            for (const Obj* last = end(d,length()); d != last; ++d)
                result += *d;
            return result;
        }

    inline Obj sumApply(Obj (*func)(const Obj&)) const {
            Obj result(0);
            const Obj* d = data();
            for (const Obj* last = end(d,length()); d != last; ++d)
                result += (*func)(*d);
            return result;
        }

    inline float sumApplyF(float (*func)(const Obj&)) const {
            float result = 0;
            const Obj* d = data();
            for (const Obj* last = end(d,length()); d != last; ++d)
                result += (*func)(*d);
            return result;
        }

    inline bool sum(const SliceVector& other) {
            if (length() != other.length())
                return false;
            const Obj* od = other.m_data;
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d, ++od)
                *d += *od;
            return true;
        }

    inline void sum(const Obj& value) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                *d += value;
        }

    inline bool sub(const SliceVector& other) {
            if (length() != other.length())
                return false;
            const Obj* od = other.m_data;
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d, ++od)
                *d -= *od;
            return true;
        }

    inline void sub(const Obj& value) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                *d -= value;
        }

    inline bool mul(const SliceVector& other) {
            if (length() != other.length())
                return false;
            const Obj* od = other.m_data;
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d, ++od)
                *d *= *od;
            return true;
        }

    inline void mul(const Obj& value) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                *d *= value;
        }

    inline void mul(float value) {
            Obj* d = data();
            for (Obj* last = end(d,length()); d != last; ++d)
                *d *= value;
        }

    inline Obj& operator[](unsigned int index) {
            YMATH_FAIL(index < m_length,
                "SliceVector::operator[] index out of bounds [%p]",this);
            return m_data[index];
        }

    inline const Obj& operator[](unsigned int index) const {
            YMATH_FAIL(index < m_length,
                "SliceVector::operator[] index out of bounds [%p]",this);
            return m_data[index];
        }

    inline Obj& operator[](signed int index) {
            YMATH_FAIL((unsigned int)index < m_length,
                "SliceVector::operator[] index out of bounds [%p]",this);
            return m_data[index];
        }

    inline const Obj& operator[](signed int index) const {
            YMATH_FAIL((unsigned int)index < m_length,
                "SliceVector::operator[] index out of bounds [%p]",this);
            return m_data[index];
        }

    inline bool operator==(const SliceVector& other) const
        { return equals(other); }

    inline bool operator!=(const SliceVector& other) const
        { return !equals(other); }

    inline SliceVector& operator=(const SliceVector& other) {
            setSlice(other);
            return *this;
        }

    inline SliceVector& operator+=(const SliceVector& other) {
            YMATH_FAIL(length() == other.length(),
                "SliceVector(+=): invalid lengths [%p]",this);
            sum(other);
            return *this;
        }

    inline SliceVector& operator+=(const Obj& value) {
            sum(value);
            return *this;
        }

    inline SliceVector& operator-=(const SliceVector& other) {
            YMATH_FAIL(length() == other.length(),
                "SliceVector(-=): invalid lengths [%p]",this);
            sub(other);
            return *this;
        }

    inline SliceVector& operator-=(const Obj& value) {
            sub(value);
            return *this;
        }

    inline SliceVector& operator*=(const SliceVector& other) {
            YMATH_FAIL(length() == other.length(),
                "SliceVector(*=): invalid lengths [%p]",this);
            mul(other);
            return *this;
        }

    inline SliceVector& operator*=(const Obj& value) {
            mul(value);
            return *this;
        }

    inline SliceVector& operator*=(float value) {
            mul(value);
            return *this;
        }

    inline bool equals(const SliceVector& other) const {
            return length() == other.length() &&
                RefStorage::equals(data(),other.data(),size());
        }

    String& dump(String& buf,
        String& (*func)(String& s, const Obj& o, const char* sep, const char* fmt),
        const char* sep = ",", const char* fmt = 0) const {
            const Obj* d = data();
            if (!(d && func))
                return buf;
            String localBuf;
            for (const Obj* last = end(d,length()); d != last; ++d)
                (*func)(localBuf,*d,sep,fmt);
            return buf.append(localBuf);
        }

    String& dump(String& buf, unsigned int lineLen,
        String& (*func)(String& s, const Obj& o, const char* sep, const char* fmt),
        unsigned int offset = 0, const char* linePrefix = 0,
        const char* suffix = "\r\n", const char* sep = ",", const char* fmt = 0) const {
            const Obj* d = data();
            if (!(d && func))
                return buf.append(suffix);
            if (TelEngine::null(linePrefix))
                linePrefix = suffix;
            if (!lineLen || TelEngine::null(linePrefix))
                return dump(buf,func,sep,fmt) << suffix;
            String localBuf;
            for (const Obj* last = end(d,length()); d != last;) {
                String tmp;
                (*func)(tmp,*d,0,fmt);
                if (++d != last)
                    tmp << sep;
                offset += tmp.length();
                if (offset > lineLen) {
                    localBuf << linePrefix;
                    offset = tmp.length();
                }
                localBuf << tmp;
            }
            return buf << localBuf << suffix;
        }

    inline String& hexify(String& buf, char sep = 0) const
        { return buf.hexify((void*)data(),size(),sep); }
        
    inline String& dumpHex(String& buf, unsigned int lineLen,
        unsigned int offset = 0, const char* linePrefix = 0,
        const char* suffix = "\r\n") const {
            String h;
            return RefStorage::dumpSplit(buf,hexify(h),lineLen,offset,linePrefix,suffix);
        }
        
    int unHexify(const char* str, unsigned int len, int sep = 255) {
            setData();
            DataBlock db;
            bool ok = (sep < -128 || sep > 127) ? db.unHexify(str,len) :
                db.unHexify(str,len,(char)sep);
            if (ok && (db.length() % objSize() == 0)) {
                initStorage(db.length() / objSize(),(const Obj*)db.data(0,db.length()));
                return 0;
            }
            return ok ? 1 : -1;
        }

    inline int unHexify(const String& str, int sep = 255)
        { return unHexify(str.c_str(),str.length(),sep); }

    static inline unsigned int objSize()
        { return sizeof(Obj); }

    static inline unsigned int size(unsigned int len)
        { return len * objSize(); }

protected:
    // Return end-of-data pointer from start and given length
    inline Obj* end(Obj* start, unsigned int len)
        { return start ? (start + len) : 0; }
    inline const Obj* end(const Obj* start, unsigned int len) const
        { return start ? (start + len) : 0; }
    // Set data. Reset storage if we don't have a valid data pointer
    // Return true if we have valid data
    inline bool setData(Obj* data = 0, unsigned int len = 0, unsigned int maxLen = 0) {
            m_data = data;
            if (m_data) {
                m_length = len;
                m_maxLen = maxLen;
            }
            else {
                m_length = m_maxLen = 0;
                TelEngine::destruct(m_storage);
            }
            return m_data != 0;
        }
    // Build storage, update data. This method assumes our data is cleared
    // If data is given 'len' elements will be copied from it to storage
    inline bool initStorage(unsigned int len, const Obj* data = 0,
        unsigned int maxLen = 0) {
            if (maxLen < len)
                maxLen = len;
            if (!maxLen)
                return false;
            if (!data || maxLen == len)
                m_storage = new RefStorage(data,size(maxLen));
            else {
                m_storage = new RefStorage(0,size(maxLen));
                m_storage->set(data,size(len));
            }
            return setData((Obj*)m_storage->data(0,1),len,maxLen);
        }
    // Build storage from slice and update data.
    // Clear data if requested
    inline bool initSlice(bool del, const SliceVector& other, unsigned int offs = 0,
        unsigned int len = 0) {
            if (!len)
                len = other.length();
            Obj* d = (Obj*)other.data(offs,len);
            if (!d) {
                if (del)
                    setData();
                return len == 0;
            }
            if (m_storage == other.m_storage)
                return setData(d,len,len);
            RefStorage* tmp = other.m_storage;
            if (tmp->ref()) {
                TelEngine::destruct(m_storage);
                m_storage = tmp;
                return setData(d,len,len);
            }
            Debug(DebugFail,"SliceVector storage ref() failed");
            return del ? setData() : false;
        }

    RefStorage* m_storage;               // Vector storage
    Obj* m_data;                         // Pointer to data
    unsigned int m_length;               // Data length
    unsigned int m_maxLen;               // Max storage
};

typedef SliceVector<Complex> ComplexVector;
typedef SliceVector<float> FloatVector;
typedef SliceVector<uint8_t> ByteVector;

class YATE_API BitVector : public ByteVector
{
public:
    inline BitVector()
        {}

    inline BitVector(const BitVector& other)
        : ByteVector(other)
        {}

    explicit inline BitVector(unsigned int len, unsigned int maxLen = 0)
        : ByteVector(len,0,maxLen)
        {}

    explicit inline BitVector(const BitVector& other, unsigned int offs,
        unsigned int len = 0)
        : ByteVector(other,offs,len)
        {}

    explicit BitVector(const char* str, unsigned int maxLen = 0);

    bool valid() const;

    bool get(FloatVector& dest) const;

    bool set(const FloatVector& input);

    void xorMsb(uint32_t value, unsigned int offs = 0, uint8_t len = 32);

    inline void xorMsb16(uint16_t value, unsigned int offs = 0, uint8_t len = 16)
        { return xorMsb((uint32_t)value << 16,offs,len <= 16 ? len : 16); }

    uint64_t pack(unsigned int offs = 0, int len = -1) const;

    void unpack(uint64_t value, unsigned int offs = 0, uint8_t len = 64);

    void unpackMsb(uint32_t value, unsigned int offs = 0, uint8_t len = 32);

    inline void unpackMsb16(uint16_t value, unsigned int offs = 0, uint8_t len = 16)
        { unpackMsb((uint32_t)value << 16,offs,len <= 16 ? len : 16); }

    bool pack(ByteVector& dest) const;

    bool unpack(const ByteVector& src);

    String& appendTo(String& buf, unsigned int offs = 0, int len = -1) const;

    inline String toString(unsigned int offs, int len = -1) const {
            String tmp;
            return appendTo(tmp,offs,len);
        }

    inline bool setSlice(const BitVector& other, unsigned int offs = 0,
        unsigned int len = 0)
        { return initSlice(true,other,offs,len); }

    inline BitVector head(unsigned int len) const
        { return slice(0,len); }

    inline bool head(BitVector& dest, unsigned int len) const
        { return slice(dest,0,len); }

    inline BitVector tail(unsigned int len) const {
            if (len < length())
                return BitVector(*this,length() - len,len);
            return BitVector();
        }

    inline bool tail(BitVector& dest, unsigned int len) const {
            if (len <= length())
                return dest.initSlice(true,*this,length() - len,len);
            dest.setData();
            return false;
        }

    inline BitVector slice(unsigned int offs, unsigned int len) const
        { return BitVector(*this,offs,len); }

    inline bool slice(BitVector& dest, unsigned int offs, unsigned int len = 0) const
        { return dest.initSlice(true,*this,offs,len); }
};


class YATE_API Math
{
public:
    static String& dumpComplex(String& buf, const Complex& val, const char* sep = 0,
        const char* fmt = 0);

    static String& dumpFloat(String& buf, const float& val, const char* sep = 0,
        const char* fmt = 0);
};


inline Complex operator+(const Complex& c1, const Complex& c2)
{
    Complex tmp(c1);
    return (tmp += c2);
}

inline Complex operator+(const Complex& c, float f)
{
    Complex tmp(c);
    return (tmp += f);
}

inline Complex operator+(float f, const Complex& c)
{
    return operator+(c,f);
}

inline Complex operator-(const Complex& c1, const Complex& c2)
{
    Complex tmp(c1);
    return (tmp -= c2);
}

inline Complex operator-(const Complex& c, float f)
{
    Complex tmp(c);
    return (tmp -= f);
}

inline Complex operator*(const Complex& c1, const Complex& c2)
{
    Complex tmp(c1);
    return (tmp *= c2);
}

inline Complex operator*(const Complex& c, float f)
{
    Complex tmp(c);
    return (tmp *= f);
}

inline Complex operator*(float f, const Complex& c)
{
    return operator*(c,f);
}

inline Complex operator/(const Complex& c1, const Complex& c2)
{
    Complex tmp(c1);
    return (tmp /= c2);
}

inline Complex operator/(const Complex& c, float f)
{
    Complex tmp(c);
    return (tmp /= f);
}

inline String& operator<<(String& str, const Complex& c)
{
    return Math::dumpComplex(str,c);
}

inline String& operator<<(String& str, const BitVector& b)
{
    return b.appendTo(str);
}

}; // namespace TelEngine

#endif /* __YATEMATH_H */

/* vi: set ts=8 sw=4 sts=4 noet: */