/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h
#define math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h

#include <gslib/config.h>
#include <gslib/basetype.h>

#ifndef PI
#define PI  (3.14159265358979323846f)
#endif

__gslib_begin__

struct _vec2 { float x, y; };
struct _vec3 { float x, y, z; };
struct _vec4 { float x, y, z, w; };
struct _mat2
{
    union
    {
        struct
        {
            float _11, _12,
                _21, _22;
        };
        float  m[2][2];
    };
};
struct _mat3
{
    union
    {
        struct
        {
            float _11, _12, _13,
                _21, _22, _23,
                _31, _32, _33;
        };
        float   m[3][3];
    };
};
struct _mat4
{
    union
    {
        struct
        {
            float _11, _12, _13, _14,
                _21, _22, _23, _24,
                _31, _32, _33, _34,
                _41, _42, _43, _44;
        };
        float   m[4][4];
    };
};
struct _quat
{
    union
    {
        struct  { float x, y, z, w; };
        float   q[4];
    };
};
struct _plane { float a, b, c, d; };

struct viewport
{
    int         left;
    int         top;
    uint        width;
    uint        height;
    float       min_depth;
    float       max_depth;
};

class vec2;
class vec3;
class vec4;
class mat3;
class mat4;
class quat;
class plane;

typedef vec2 point2;
typedef vec3 point3;
typedef mat4 matrix;
typedef mat3 matrix3;
typedef _mat2 matrix2;

/* inline */
float vec2length(const vec2* p);
float vec2lengthsq(const vec2* p);
float vec2dot(const vec2* p1, const vec2* p2);
float vec2ccw(const vec2* p1, const vec2* p2);
vec2* vec2add(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2sub(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2min(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2max(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2scale(vec2* out, const vec2* p, float s);
vec2* vec2lerp(vec2* out, const vec2* p1, const vec2* p2, float s);
float vec2lerp_x(const vec2* p1, const vec2* p2, float y);
float vec2lerp_y(const vec2* p1, const vec2* p2, float x);
vec2* vec2multiply(vec2* out, const vec2* p, const matrix2* m);
vec2* vec2transformcoord(vec2* out, const vec2* p, const mat3* m);
vec2* vec2transformnormal(vec2* out, const vec2* p, const mat3* m);
float vec3length(const vec3* v);
float vec3lengthsq(const vec3* v);
float vec3dot(const vec3* v1, const vec3* v2);
vec3* vec3cross(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3add(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3sub(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3min(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3max(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3scale(vec3* out, const vec3* v, float s);
vec3* vec3lerp(vec3* out, const vec3* v1, const vec3* v2, float s);
vec3* vec3multiply(vec3* out, const vec3* v, const mat3* m);
vec3* vec3project(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3unproject(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3projectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
vec3* vec3unprojectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
float vec4length(const vec4* v);
float vec4lengthsq(const vec4* v);
float vec4dot(const vec4* v1, const vec4* v2);
vec4* vec4add(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4sub(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4min(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4max(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4scale(vec4* out, const vec4* v, float s);
vec4* vec4lerp(vec4* out, const vec4* v1, const vec4* v2, float s);
vec4* vec4multiply(vec4* out, const vec4* v, const matrix* m);
matrix3* mat3identity(matrix3* out);
bool mat3isidentity(const matrix3* m);
float mat3determinant(const matrix3* m);
matrix3* mat3inverse(matrix3* out, float* det, const matrix3* m);
matrix3* mat3multiply(matrix3* out, const matrix3* m1, const matrix3* m2);
matrix3* mat3transpose(matrix3* out, const matrix3* m);
matrix3* mat3scaling(matrix3* out, float sx, float sy);
matrix3* mat3translation(matrix3* out, float x, float y);
matrix3* mat3rotation(matrix3* out, float angle);
matrix3* mat3rotationpos(matrix3* out, const vec2* p, float angle);
matrix* matidentity(matrix* out);
bool matisidentity(const matrix* m);
bool matdecompose(vec3* scale, quat* rot, vec3* trans, const matrix* m);
matrix* mattranspose(matrix* out, const matrix* m);
matrix* matscaling(matrix* out, float sx, float sy, float sz);
matrix* mattranslation(matrix* out, float x, float y, float z);
matrix* matrotation_x(matrix* out, float angle);
matrix* matrotation_y(matrix* out, float angle);
matrix* matrotation_z(matrix* out, float angle);
matrix* matrotationaxis(matrix* out, const vec3* v, float angle);
matrix* matrotationquatern(matrix* out, const quat* q);
matrix* matrotationeuler(matrix* out, float yaw, float pitch, float roll);
matrix* matlookatlh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matlookatrh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matperspectivelh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectiverh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectivefovlh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectivefovrh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectiveoffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matperspectiveoffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matortholh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthorh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthooffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matorthooffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matviewportproject(matrix* out, const viewport* vp);
matrix* matviewportunproject(matrix* out, const viewport* vp);
matrix* mattransform(matrix* out, const vec3* scalecenter, const quat* scalerot, const vec3* scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mattransform2d(matrix* out, const vec2* scalecenter, float scalerot, const vec2* scale, const vec2* rotcenter, float rot, const vec2* trans);
matrix* mataffinetransform(matrix* out, float scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mataffinetransform2d(matrix* out, float scale, const vec2* rotcenter, float rot, const vec2* trans);
float quatlength(const quat* q);
float quatlengthsq(const quat* q);
float quatdot(const quat* q1, const quat* q2);
quat* quatidentity(quat* out);
bool quatisidenetity(const quat* q);
quat* quatconjugate(quat* out, const quat* q);
void quattoaxisangle(const quat* q, vec3* axis, float* angle);
quat* quatrotatematrix(quat* out, const matrix* m);
quat* quatrotateaxis(quat* out, const vec3* v, float angle);
quat* quatln(quat* out, const quat* q);
quat* quatexp(quat* out, const quat* q);
quat* quatslerp(quat* out, const quat* q1, const quat* q2, float t);
quat* quatsquad(quat* out, const quat* q, const quat* a, const quat* b, const quat* c, float t);
quat* quatbarycentric(quat* out, const quat* q1, const quat* q2, const quat* q3, float f, float g);
void quatsquadsetup(quat* a, quat* b, quat* c, const quat* q1, const quat* q2, const quat* q3, const quat* q4);
float planedot(const plane* p, const vec4* v);
float planedotcoord(const plane* p, const vec3* v);
float planedotnormal(const plane* p, const vec3* v);
plane* planescale(plane* out, const plane* p, float s);
plane* planefrompointnormal(plane* out, const vec3* p, const vec3* normal);

/* non-inline */
typedef vec2* (__stdcall *fnvec2normalize)(vec2* out, const vec2* v);
typedef vec2* (__stdcall *fnvec2hermite)(vec2* out, const vec2* p1, const vec2* t1, const vec2* p2, const vec2* t2, float s);
typedef vec2* (__stdcall *fnvec2catmullrom)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, const vec2* p4, float s);
typedef vec2* (__stdcall *fnvec2barycentric)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, float f, float g);
typedef vec4* (__stdcall *fnvec2transform)(vec4* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformcoord)(vec2* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformnormal)(vec2* out, const vec2* p, const matrix* m);
typedef vec4* (__stdcall *fnvec2transformarray)(vec4* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformcoordarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformnormalarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3normalize)(vec3* out, const vec3* v);
typedef vec3* (__stdcall *fnvec3hermite)(vec3* out, const vec3* v1, const vec3* t1, const vec3* v2, const vec3* t2, float s);
typedef vec3* (__stdcall *fnvec3catmullrom)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, const vec3* v4, float s);
typedef vec3* (__stdcall *fnvec3barycentric)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, float f, float g);
typedef vec4* (__stdcall *fnvec3transform)(vec4* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformcoord)(vec3* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformnormal)(vec3* out, const vec3* v, const matrix* m);
typedef vec4* (__stdcall *fnvec3transformarray)(vec4* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformcoordarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformnormalarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec4* (__stdcall *fnvec4cross)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3);
typedef vec4* (__stdcall *fnvec4normalize)(vec4* out, const vec4* v);
typedef vec4* (__stdcall *fnvec4hermite)(vec4* out, const vec4* v1, const vec4* t1, const vec4* v2, const vec4* t2, float s);
typedef vec4* (__stdcall *fnvec4catmullrom)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, const vec4* v4, float s);
typedef vec4* (__stdcall *fnvec4barycentric)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, float f, float g);
typedef vec4* (__stdcall *fnvec4transform)(vec4* out, const vec4* v, const matrix* m);
typedef vec4* (__stdcall *fnvec4transformarray)(vec4* out, uint ostride, const vec4* v, uint vstride, const matrix* m, uint n);
typedef float (__stdcall *fnmatdeterminant)(const matrix* m);
typedef matrix* (__stdcall *fnmatmultiply)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatmultiplytranspose)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatinverse)(matrix* out, float* determinant, const matrix* m);
typedef matrix* (__stdcall *fnmatshadow)(matrix* out, const vec4* plight, const plane* pln);
typedef matrix* (__stdcall *fnmatreflect)(matrix* out ,const plane* pln);
typedef quat* (__stdcall *fnquatrotateeuler)(quat* out, float yaw, float pitch, float roll);
typedef quat* (__stdcall *fnquatmultiply)(quat* out, const quat* q1, const quat* q2);
typedef quat* (__stdcall *fnquatnormalize)(quat* out, const quat* q);
typedef quat* (__stdcall *fnquatinverse)(quat* out, const quat* q);
typedef plane* (__stdcall *fnplanenormalize)(plane* out, const plane* p);
typedef vec3* (__stdcall *fnplaneintersectline)(vec3* out, const plane* p, const vec3* v1, const vec3* v2);
typedef plane* (__stdcall *fnplanefrompoints)(plane* out, const vec3* v1, const vec3* v2, const vec3* v3);
typedef plane* (__stdcall *fnplanetransform)(plane* out, const plane* p, const matrix* m);
typedef plane* (__stdcall *fnplanetransformarray)(plane* out, uint ostride, const plane* p, uint pstride, const matrix* m, uint n);

gs_export extern fnvec2normalize vec2normalize;
gs_export extern fnvec2hermite vec2hermite;
gs_export extern fnvec2catmullrom vec2catmullrom;
gs_export extern fnvec2barycentric vec2barycentric;
gs_export extern fnvec2transform vec2transform;
gs_export extern fnvec2transformarray vec2transformarray;
gs_export extern fnvec2transformcoordarray vec2transformcoordarray;
gs_export extern fnvec2transformnormalarray vec2transformnormalarray;
gs_export extern fnvec3normalize vec3normalize;
gs_export extern fnvec3hermite vec3hermite;
gs_export extern fnvec3catmullrom vec3catmullrom;
gs_export extern fnvec3barycentric vec3barycentric;
gs_export extern fnvec3transform vec3transform;
gs_export extern fnvec3transformcoord vec3transformcoord;
gs_export extern fnvec3transformnormal vec3transformnormal;
gs_export extern fnvec3transformarray vec3transformarray;
gs_export extern fnvec3transformcoordarray vec3transformcoordarray;
gs_export extern fnvec3transformnormalarray vec3transformnormalarray;
gs_export extern fnvec4cross vec4cross;
gs_export extern fnvec4normalize vec4normalize;
gs_export extern fnvec4hermite vec4hermite;
gs_export extern fnvec4catmullrom vec4catmullrom;
gs_export extern fnvec4barycentric vec4barycentric;
gs_export extern fnvec4transform vec4transform;
gs_export extern fnvec4transformarray vec4transformarray;
gs_export extern fnmatdeterminant matdeterminant;
gs_export extern fnmatmultiply matmultiply;
gs_export extern fnmatmultiplytranspose matmultiplytranspose;
gs_export extern fnmatinverse matinverse;
gs_export extern fnmatshadow matshadow;
gs_export extern fnmatreflect matreflect;
gs_export extern fnquatrotateeuler quatrotateeuler;
gs_export extern fnquatmultiply quatmultiply;
gs_export extern fnquatnormalize quatnormalize;
gs_export extern fnquatinverse quatinverse;
gs_export extern fnplanenormalize planenormalize;
gs_export extern fnplaneintersectline planeintersectline;
gs_export extern fnplanefrompoints planefrompoints;
gs_export extern fnplanetransform planetransform;
gs_export extern fnplanetransformarray planetransformarray;

class gs_export vec2:
    public _vec2
{
public:
    vec2() {}
    vec2(const float*);
    vec2(float x, float y);

public:
    operator float*();
    operator const float*() const;

public:
    vec2& operator+=(const vec2&);
    vec2& operator-=(const vec2&);
    vec2& operator*=(float);
    vec2& operator/=(float);

public:
    vec2 operator+() const;
    vec2 operator-() const;

public:
    vec2 operator+(const vec2&) const;
    vec2 operator-(const vec2&) const;
    vec2 operator*(float) const;
    vec2 operator/(float) const;
    friend vec2 operator*(float, const vec2&);

public:
    bool operator==(const vec2&) const;
    bool operator!=(const vec2&) const;

public:
    float length() const { return vec2length(this); }
    float lengthsq() const { return vec2lengthsq(this); }
    float dot(const vec2& v) const { return vec2dot(this, &v); }
    float ccw(const vec2& v) const { return vec2ccw(this, &v); }
    vec2& add(const vec2& p1, const vec2& p2) { return *vec2add(this, &p1, &p2); }
    vec2& sub(const vec2& p1, const vec2& p2) { return *vec2sub(this, &p1, &p2); }
    vec2& scale(float s) { return *vec2scale(this, this, s); }
    vec2& scale(const vec2& v, float s) { return *vec2scale(this, &v, s); }
    vec2& lerp(const vec2& p1, const vec2& p2, float s) { return *vec2lerp(this, &p1, &p2, s); }
    vec2& multiply(const matrix2& m) { return *vec2multiply(this, this, &m); }
    vec2& multiply(const vec2& p, const matrix2& m) { return *vec2multiply(this, &p, &m); }
    vec2& normalize() { return *vec2normalize(this, this); }
    vec2& normalize(const vec2& v) { return *vec2normalize(this, &v); }
    vec2& hermite(const vec2& p1, const vec2& t1, const vec2& p2, const vec2& t2, float s) { return *vec2hermite(this, &p1, &t1, &p2, &t2, s); }
    vec2& catmullrom(const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4, float s) { return *vec2catmullrom(this, &p1, &p2, &p3, &p4, s); }
    vec2& barycentric(const vec2& p1, const vec2& p2, const vec2& p3, float f, float g) { return *vec2barycentric(this, &p1, &p2, &p3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec2transform(&v, this, &m); }
    vec2& transformcoord(const mat3& m) { return *vec2transformcoord(this, this, &m); }
    vec2& transformcoord(const vec2& p, const mat3& m) { return *vec2transformcoord(this, &p, &m); }
    vec2& transformnormal(const mat3& m) { return *vec2transformnormal(this, this, &m); }
    vec2& transformnormal(const vec2& p, const mat3& m) { return *vec2transformnormal(this, &p, &m); }
};

class gs_export vec3:
    public _vec3
{
public:
    vec3() {}
    vec3(const float*);
    vec3(float x, float y, float z);

public:
    operator float*();
    operator const float*() const;

public:
    vec3& operator+=(const vec3&);
    vec3& operator-=(const vec3&);
    vec3& operator*=(float);
    vec3& operator/=(float);

public:
    vec3 operator+() const;
    vec3 operator-() const;

public:
    vec3 operator+(const vec3&) const;
    vec3 operator-(const vec3&) const;
    vec3 operator*(float) const;
    vec3 operator/(float) const;
    friend vec3 operator*(float, const vec3&);

public:
    bool operator==(const vec3&) const;
    bool operator!=(const vec3&) const;

public:
    float length() const { return vec3length(this); }
    float lengthsq() const { return vec3lengthsq(this); }
    float dot(const vec3& v) const { return vec3dot(this, &v); }
    vec3& add(const vec3& v1, const vec3& v2) { return *vec3add(this, &v1, &v2); }
    vec3& sub(const vec3& v1, const vec3& v2) { return *vec3sub(this, &v1, &v2); }
    vec3& cross(const vec3& v) { return *vec3cross(this, &vec3(*this), &v); }
    vec3& cross(const vec3& v1, const vec3& v2) { return *vec3cross(this, &v1, &v2); }
    vec3& scale(float s) { return *vec3scale(this, this, s); }
    vec3& scale(const vec3& v, float s) { return *vec3scale(this, &v, s); }
    vec3& lerp(const vec3& p1, const vec3& p2, float s) { return *vec3lerp(this, &p1, &p2, s); }
    vec3& multiply(const vec3& p, const mat3& m) { return *vec3multiply(this, &p, &m); }
    vec3& project(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3project(this, this, &vp, &prj, &view, &world); }
    vec3& unproject(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3unproject(this, this, &vp, &prj, &view, &world); }
    vec3& normalize() { return *vec3normalize(this, this); }
    vec3& normalize(const vec3& v) { return *vec3normalize(this, &v); }
    vec3& hermite(const vec3& v1, const vec3& t1, const vec3& v2, const vec3& t2, float s) { return *vec3hermite(this, &v1, &t1, &v2, &t2, s); }
    vec3& catmullrom(const vec3& v1, const vec3& v2, const vec3& v3, const vec3& v4, float s) { return *vec3catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec3& barycentric(const vec3& v1, const vec3& v2, const vec3& v3, float f, float g) { return *vec3barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec3transform(&v, this, &m); }
    vec3& transformcoord(const matrix& m) { return *vec3transformcoord(this, this, &m); }
    vec3& transformcoord(const vec3& v, const matrix& m) { return *vec3transformcoord(this, &v, &m); }
    vec3& transformnormal(const matrix& m) { return *vec3transformnormal(this, this, &m); }
    vec3& transformnormal(const vec3& v, const matrix& m) { return *vec3transformnormal(this, &v, &m); }
};

class gs_export vec4:
    public _vec4
{
public:
    vec4() {}
    vec4(const float*);
    vec4(const _vec3& xyz, float w);
    vec4(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    vec4& operator+=(const vec4&);
    vec4& operator-=(const vec4&);
    vec4& operator*=(float);
    vec4& operator/=(float);

public:
    vec4 operator+() const;
    vec4 operator-() const;

public:
    vec4 operator+(const vec4&) const;
    vec4 operator-(const vec4&) const;
    vec4 operator*(float) const;
    vec4 operator/(float) const;
    friend vec4 operator*(float, const vec4&);

public:
    bool operator==(const vec4&) const;
    bool operator!=(const vec4&) const;

public:
    float length() const { return vec4length(this); }
    float lengthsq() const { return vec4lengthsq(this); }
    float dot(const vec4& v) const { return vec4dot(this, &v); }
    vec4& add(const vec4& v1, const vec4& v2) { return *vec4add(this, &v1, &v2); }
    vec4& sub(const vec4& v1, const vec4& v2) { return *vec4sub(this, &v1, &v2); }
    vec4& scale(float s) { return *vec4scale(this, this, s); }
    vec4& scale(const vec4& v, float s) { return *vec4scale(this, &v, s); }
    vec4& lerp(const vec4& v1, const vec4& v2, float s) { return *vec4lerp(this, &v1, &v2, s); }
    vec4& multiply(const matrix& m) { return *vec4multiply(this, this, &m); }
    vec4& multiply(const vec4& v, const matrix& m) { return *vec4multiply(this, &v, &m); }
    vec4& cross(const vec4& v1, const vec4& v2, const vec4& v3) { return *vec4cross(this, &v1, &v2, &v3); }
    vec4& hermite(const vec4& v1, const vec4& t1, const vec4& v2, const vec4& t2, float s) { return *vec4hermite(this, &v1, &t1, &v2, &t2, s); }
    vec4& catmullrom(const vec4& v1, const vec4& v2, const vec4& v3, const vec4& v4, float s) { return *vec4catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec4& barycentric(const vec4& v1, const vec4& v2, const vec4& v3, float f, float g) { return *vec4barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(const matrix& m) { return *vec4transform(this, this, &m); }
    vec4& transform(const vec4& v, const matrix& m) { return *vec4transform(this, &v, &m); }
};

class gs_export mat3:
    public _mat3
{
public:
    mat3() {}
    mat3(const float*);
    mat3(const mat3&);
    mat3(float f11, float f12, float f13,
         float f21, float f22, float f23,
         float f31, float f32, float f33
         );

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat3& operator*=(const mat3&);
    mat3& operator+=(const mat3&);
    mat3& operator-=(const mat3&);
    mat3& operator*=(float);
    mat3& operator/=(float);

public:
    mat3 operator+() const;
    mat3 operator-() const;

public:
    mat3 operator*(const mat3&) const;
    mat3 operator+(const mat3&) const;
    mat3 operator-(const mat3&) const;
    mat3 operator*(float) const;
    mat3 operator/(float) const;
    friend mat3 operator*(float, const mat3&);

public:
    bool operator==(const mat3&) const;
    bool operator!=(const mat3&) const;

public:
    mat3& identity() { return *mat3identity(this); }
    bool isidentity() const { return mat3isidentity(this); }
    mat3& transpose() { return *mat3transpose(this, this); }
    mat3& transpose(const mat3& m) { return *mat3transpose(this, &m); }
    mat3& scaling(float sx, float sy) { return *mat3scaling(this, sx, sy); }
    mat3& translation(float x, float y) { return *mat3translation(this, x, y); }
    mat3& rotation(float angle) { return *mat3rotation(this, angle); }
    mat3& rotationpos(const vec2& p, float angle) { return *mat3rotationpos(this, &p, angle); }
    mat3& multiply(const mat3& m) { return *mat3multiply(this, this, &m); }
    mat3& multiply(const mat3& m1, const mat3& m2) { return *mat3multiply(this, &m1, &m2); }
    float determinant() const { return mat3determinant(this); }
    mat3& inverse(float* det, const mat3& m) { return *mat3inverse(this, det, &m); }
};

class gs_export mat4:
    public _mat4
{
public:
    mat4() {}
    mat4(const float*);
    mat4(const mat4&);
    mat4(float f11, float f12, float f13, float f14,
         float f21, float f22, float f23, float f24,
         float f31, float f32, float f33, float f34,
         float f41, float f42, float f43, float f44);

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat4& operator*=(const mat4&);
    mat4& operator+=(const mat4&);
    mat4& operator-=(const mat4&);
    mat4& operator*=(float);
    mat4& operator/=(float);

public:
    mat4 operator+() const;
    mat4 operator-() const;

public:
    mat4 operator*(const mat4&) const;
    mat4 operator+(const mat4&) const;
    mat4 operator-(const mat4&) const;
    mat4 operator*(float) const;
    mat4 operator/(float) const;
    friend mat4 operator*(float, const mat4&);

public:
    bool operator==(const mat4&) const;
    bool operator!=(const mat4&) const;

public:
    mat4& identity() { return *matidentity(this); }
    bool isidentity() const { return matisidentity(this); }
    bool decompose(vec3& scale, quat& rot, vec3& trans) { return matdecompose(&scale, &rot, &trans, this); }
    mat4& transpose() { return *mattranspose(this, this); }
    mat4& transpose(const mat4& m) { return *mattranspose(this, &m); }
    mat4& scaling(float sx, float sy, float sz) { return *matscaling(this, sx, sy, sz); }
    mat4& translation(float x, float y, float z) { return *mattranslation(this, x, y, z); }
    mat4& rotation_x(float angle) { return *matrotation_x(this, angle); }
    mat4& rotation_y(float angle) { return *matrotation_y(this, angle); }
    mat4& rotation_z(float angle) { return *matrotation_z(this, angle); }
    mat4& rotation(const vec3& v, float angle) { return *matrotationaxis(this, &v, angle); }
    mat4& rotation(const quat& q) { return *matrotationquatern(this, &q); }
    mat4& rotation(float yaw, float pitch, float roll) { return *matrotationeuler(this, yaw, pitch, roll); }
    mat4& lookatlh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatlh(this, &eye, &at, &up); }
    mat4& lookatrh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatrh(this, &eye, &at, &up); }
    mat4& perspectivelh(float w, float h, float zn, float zf) { return *matperspectivelh(this, w, h, zn, zf); }
    mat4& perspectiverh(float w, float h, float zn, float zf) { return *matperspectiverh(this, w, h, zn, zf); }
    mat4& perspectivefovlh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovlh(this, fovy, aspect, zn, zf); }
    mat4& perspectivefovrh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovrh(this, fovy, aspect, zn, zf); }
    mat4& perspectiveoffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& perspectiveoffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& ortholh(float w, float h, float zn, float zf) { return *matortholh(this, w, h, zn, zf); }
    mat4& orthorh(float w, float h, float zn, float zf) { return *matorthorh(this, w, h, zn, zf); }
    mat4& orthooffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterlh(this, l, r, b, t, zn, zf); }
    mat4& orthooffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterrh(this, l, r, b, t, zn, zf); }
    mat4& viewportproject(const viewport& vp) { return *matviewportproject(this, &vp); }
    mat4& viewportunproject(const viewport& vp) { return *matviewportunproject(this, &vp); }
    mat4& transform(const vec3& scalecenter, const quat& scalerot, const vec3& scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mattransform(this, &scalecenter, &scalerot, &scale, &rotcenter, &rot, &trans); }
    mat4& transform2d(const vec2& scalecenter, float scalerot, const vec2& scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mattransform2d(this, &scalecenter, scalerot, &scale, &rotcenter, rot, &trans); }
    mat4& affinetransform(float scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mataffinetransform(this, scale, &rotcenter, &rot, &trans); }
    mat4& affinetransform2d(float scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mataffinetransform2d(this, scale, &rotcenter, rot, &trans); }
    float determinant() const { return matdeterminant(this); }
    mat4& multiply(const mat4& m) { return *matmultiply(this, this, &m); }
    mat4& multiply(const mat4& m1, const mat4& m2) { return *matmultiply(this, &m1, &m2); }
    mat4& multiplytranspose(const mat4& m) { return *matmultiplytranspose(this, this, &m); }
    mat4& multiplytranspose(const mat4& m1, const mat4& m2) { return *matmultiplytranspose(this, &m1, &m2); }
    mat4& inverse(float* determinant, const mat4& m) { return *matinverse(this, determinant, &m); }
    mat4& shadow(const vec4& plight, const plane& pln) { return *matshadow(this, &plight, &pln); }
    mat4& reflect(const plane& pln) { return *matreflect(this, &pln); }
};

class gs_export quat:
    public _quat
{
public:
    quat() {}
    quat(const float*);
    quat(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    quat& operator+=(const quat&);
    quat& operator-=(const quat&);
    quat& operator*=(const quat&);
    quat& operator*=(float);
    quat& operator/=(float);

public:
    quat operator+() const;
    quat operator-() const;

public:
    quat operator+(const quat&) const;
    quat operator-(const quat&) const;
    quat operator*(const quat&) const;
    quat operator*(float) const;
    quat operator/(float) const;
    friend quat operator*(float, const quat&);

public:
    bool operator==(const quat&) const;
    bool operator!=(const quat&) const;

public:
    float length() const { return quatlength(this); }
    float lengthsq() const { return quatlengthsq(this); }
    float dot(const quat& q) const { return quatdot(this, &q); }
    quat& identity() { return *quatidentity(this); }
    bool isidentity() const { return quatisidenetity(this); }
    quat& conjugate() { return *quatconjugate(this, this); }
    quat& conjugate(const quat& q) { return *quatconjugate(this, &q); }
    float toaxisangle(vec3& axis) const
    {
        float f;
        quattoaxisangle(this, &axis, &f);
        return f;
    }
    quat& rotate(const matrix& m) { return *quatrotatematrix(this, &m); }
    quat& rotate(const vec3& v, float angle) { return *quatrotateaxis(this, &v, angle); }
    quat& ln() { return *quatln(this, this); }
    quat& ln(const quat& q) { return *quatln(this, &q); }
    quat& exp() { return *quatexp(this, this); }
    quat& exp(const quat& q) { return *quatexp(this, &q); }
    quat& slerp(const quat& q1, const quat& q2, float t) { return *quatslerp(this, &q1, &q2, t); }
    quat& squad(const quat& q, const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, &q, &a, &b, &c, f); }
    quat& squad(const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, this, &a, &b, &c, f); }
    quat& barycentric(const quat& q1, const quat& q2, const quat& q3, float f, float g) { return *quatbarycentric(this, &q1, &q2, &q3, f, g); }
    quat& rotate(float yaw, float pitch, float roll) { return *quatrotateeuler(this, yaw, pitch, roll); }
    quat& multiply(const quat& q1, const quat& q2) { return *quatmultiply(this, &q1, &q2); }
    quat& normalize() { return *quatnormalize(this, this); }
    quat& normalize(const quat& q) { return *quatnormalize(this, &q); }
    quat& inverse() { return *quatinverse(this, this); }
    quat& inverse(const quat& q) { return *quatinverse(this, &q); }
};

class gs_export plane:
    public _plane
{
public:
    plane() {}
    plane(const float*);
    plane(float a, float b, float c, float d);

public:
    operator float*();
    operator const float*() const;

public:
    plane& operator*=(float);
    plane& operator/=(float);
    plane operator+() const;
    plane operator-() const;
    plane operator*(float) const;
    plane operator/(float) const;
    friend plane operator*(float, const plane&);

public:
    bool operator==(const plane&) const;
    bool operator!=(const plane&) const;

public:
    float dot(const vec4& v) const { return planedot(this, &v); }
    float dotcoord(const vec3& v) const { return planedotcoord(this, &v); }
    float dotnormal(const vec3& v) const { return planedotnormal(this, &v); }
    plane& scale(float s) { return *planescale(this, this, s); }
    plane& scale(const plane& p, float s) { return *planescale(this, &p, s); }
    plane& frompointnormal(const vec3& p, const vec3& normal) { return *planefrompointnormal(this, &p, &normal); }
    plane& normalize() { return *planenormalize(this, this); }
    plane& normalize(const plane& p) { return *planenormalize(this, &p); }
    vec3& intersectline(vec3& v, const vec3& v1, const vec3& v2) const { return *planeintersectline(&v, this, &v1, &v2); }
    plane& frompoints(const vec3& v1, const vec3& v2, const vec3& v3) { return *planefrompoints(this, &v1, &v2, &v3); }
    plane& transform(const matrix& m) { return *planetransform(this, this, &m); }
    plane& transform(const plane& p, const matrix& m) { return *planetransform(this, &p, &m); }
};

__gslib_end__

#include <gslib/math.inl>

#endif


// @file ker2col_conv.c
//
//  \date Created on: Sep 24, 2017
//  \author Gopalakrishna Hegde
//
//   Description:
//
//
//

#include <stdbool.h>
#include <cblas.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include "common_types.h"
#include "data_reshape.h"
#include "utils.h"
#include "conv_layers.h"

bool Kn2ColConvLayer(const float *in_data, const float *filters,
                         const float *bias, TensorDim in_dim,
                         TensorDim filt_dim, int stride, int pad, int group,
                         float *output) {
  // Currently we have limited support.
  assert(group == 1);
  assert((pad == 0) || (pad == filt_dim.w / 2));
  assert(in_dim.n == 1);
  assert(filt_dim.h == filt_dim.w);
  assert(stride == 1);

  // Output dimensions.
  TensorDim out_dim;
  out_dim.w = (in_dim.w + (pad + pad) - filt_dim.w) / stride + 1;
  out_dim.h = (in_dim.h + (pad + pad) - filt_dim.h) / stride + 1;
  out_dim.c = filt_dim.n;
  out_dim.n = in_dim.n;

  // Reshape filters in CHWN (ker_size x ker_size x no_in_ch x no_out_ch) format
  float *kkcm_filters = malloc(filt_dim.n * filt_dim.c * filt_dim.h *
                               filt_dim.w * sizeof(float));
  NCHW2HWCN(filters, filt_dim.n, filt_dim.c, filt_dim.h, filt_dim.w,
            kkcm_filters);

  // Just for convenience
  int H = in_dim.h;
  int W = in_dim.w;
  float alpha = 1.0;
  float beta = 0.0;

  // We need separate buffer because GEMM output will have width = H*W even
  // if there is no padding (pad = 0).
  float *gemm_output = malloc(out_dim.c * H * W * sizeof(float));
  float *nchw_gemm_output = malloc(out_dim.c * H * W * sizeof(float));
  // Prefill output buffer with bias if present else set to zero.
  if (bias) {
    for (int m = 0; m < out_dim.c; ++m) {
      for (int a = 0; a < out_dim.h * out_dim.w; ++a) {
        output[m * out_dim.h * out_dim.w + a] = bias[m];
      }
      // For batch size > 1
      for (int b = 1; b < out_dim.n; ++b) {
        memcpy(output + b * out_dim.c * out_dim.h * out_dim.w,
               output, out_dim.c * out_dim.h * out_dim.w * sizeof(float));
      }
    }
  } else {
    memset(output, 0, out_dim.n * out_dim.c * out_dim.h * out_dim.w *
           sizeof(float));
  }

  for (int kr = 0; kr < filt_dim.h; kr++) {
    int row_shift = pad - kr;
    for (int kc = 0; kc < filt_dim.w; kc++) {
      int group_no = kr * filt_dim.w + kc;
      int col_shift = pad - kc;
      // Matrix dimensions - A -> mxk B -> kxn  C --> mxn
      int m = in_dim.h * in_dim.w;
      int k = filt_dim.c;
      int n = filt_dim.n;

      // output is in H x W x C format.
      cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                  m, n, k, alpha, in_data, m,
                  kkcm_filters + group_no * filt_dim.c * filt_dim.n, n, beta,
                  gemm_output, n);

      // convert to CxHxW format.
      // FIXME: this will be slow. Need to find other ways :(
      NHWC2NCHW(gemm_output, 1, filt_dim.n, H, W, nchw_gemm_output);

      for (int omap = 0; omap < filt_dim.n; omap++) {
        MatrixShiftAdd(output + omap * out_dim.h * out_dim.w,
                        out_dim.h, out_dim.w,
                        nchw_gemm_output + omap * H * W,
                        H, W, row_shift, col_shift);
      }
    }
  }
  free(kkcm_filters);
  free(gemm_output);
  free(nchw_gemm_output);
  return true;
}


/* histogram/pdf.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007, 2009 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

#include "find.c"

double
gsl_histogram_pdf_sample (const gsl_histogram_pdf * p, double r)
{
  size_t i;
  int status;

/* Wrap the exclusive top of the bin down to the inclusive bottom of
   the bin. Since this is a single point it should not affect the
   distribution. */

  if (r == 1.0)
    {
      r = 0.0;
    }

  status = find (p->n, p->sum, r, &i);

  if (status)
    {
      GSL_ERROR_VAL ("cannot find r in cumulative pdf", GSL_EDOM, 0);
    }
  else
    {
      double delta = (r - p->sum[i]) / (p->sum[i + 1] - p->sum[i]);
      double x = p->range[i] + delta * (p->range[i + 1] - p->range[i]);
      return x;
    }
}

gsl_histogram_pdf *
gsl_histogram_pdf_alloc (const size_t n)
{
  gsl_histogram_pdf *p;

  if (n == 0)
    {
      GSL_ERROR_VAL ("histogram pdf length n must be positive integer",
                        GSL_EDOM, 0);
    }

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

  if (p == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for histogram pdf struct",
                        GSL_ENOMEM, 0);
    }

  p->range = (double *) malloc ((n + 1) * sizeof (double));

  if (p->range == 0)
    {
      free (p);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for histogram pdf ranges",
                        GSL_ENOMEM, 0);
    }

  p->sum = (double *) malloc ((n + 1) * sizeof (double));

  if (p->sum == 0)
    {
      free (p->range);
      free (p);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for histogram pdf sums",
                        GSL_ENOMEM, 0);
    }

  p->n = n;

  return p;
}

int
gsl_histogram_pdf_init (gsl_histogram_pdf * p, const gsl_histogram * h)
{
  size_t i;
  size_t n = p->n;

  if (n != h->n)
    {
      GSL_ERROR ("histogram length must match pdf length", GSL_EINVAL);
    }

  for (i = 0; i < n; i++)
    {
      if (h->bin[i] < 0)
        {
          GSL_ERROR ("histogram bins must be non-negative to compute"
                     "a probability distribution", GSL_EDOM);
        }
    }

  for (i = 0; i < n + 1; i++)
    {
      p->range[i] = h->range[i];
    }

  {
    double mean = 0, sum = 0;

    for (i = 0; i < n; i++)
      {
        mean += (h->bin[i] - mean) / ((double) (i + 1));
      }

    p->sum[0] = 0;

    for (i = 0; i < n; i++)
      {
        sum += (h->bin[i] / mean) / n;
        p->sum[i + 1] = sum;
      }
  }

  return GSL_SUCCESS;
}

void
gsl_histogram_pdf_free (gsl_histogram_pdf * p)
{
  RETURN_IF_NULL (p);
  free (p->range);
  free (p->sum);
  free (p);
}


/*
  code to do weak lensing power spectrum computations

  Matthew Becker, UofC 2011
*/

#include <gsl/gsl_spline.h>

#ifndef _WEAKLENS_
#define _WEAKLENS_

typedef struct {
  int wlNum;
  double lmin;
  double lmax;
  double tmin;
  double tmax;
} weaklensData;

extern weaklensData wlData;

typedef struct {
  int initFlag;
  int currCosmoNum;
  int currWLNum;
  double zs1;
  double zs2;
  double chis1;
  double chis2;
  double chiLim;
  double sn;
  double ell; //temp var for integrals
  gsl_spline *spline;
  gsl_interp_accel *accel;
} _lensPowerSpectra;

typedef _lensPowerSpectra *lensPowerSpectra;

typedef struct {
  int initFlag;
  int currCosmoNum;
  int currWLNum;
  lensPowerSpectra lps;
  double theta; //temp var for integrals
  gsl_spline *splineP;
  gsl_interp_accel *accelP;
  gsl_spline *splineM;
  gsl_interp_accel *accelM;
} _lensCorrFunc;

typedef _lensCorrFunc * lensCorrFunc;

//functions to compute lensing power and cross spectra
double nonlinear_powspec_for_lens(double k, double a);
double lens_power_spectrum(double ell, lensPowerSpectra lps);
lensPowerSpectra init_lens_power_spectrum(double zs1, double zs2);
void free_lens_power_spectrum(lensPowerSpectra lps);

//functions to compute lens corr func.
double lens_corr_func_minus(double theta, lensCorrFunc lcf);
double lens_corr_func_plus(double theta, lensCorrFunc lcf);
lensCorrFunc init_lens_corr_func(lensPowerSpectra lps);
void free_lens_corr_func(lensCorrFunc lcf);

#endif /* _WEAKLENS_ */


/*
** Copyright (C) 2004 Jonathan G. Underwood <j.underwood@open.ac.uk>
**
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation; either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/

/* Things to check: Not sure I'm using the correct error macros - should I be
   using the ones in the GSL manual? Based the ones here on what i see in
   coupling.c. */

#include <config.h>
#include <stdlib.h>
#include <error.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_wigner.h>

/* Static prototypes. */
static int istriangle (const int two_ja, const int two_jb, const int two_jc);
static int lndelta_e (const int two_j1, const int two_j2, const int two_j3,
		      gsl_sf_result * lndelta);

/* This macro returns 1 if n is even, -1 if n is odd. */
#define PHASE(n) (GSL_IS_ODD(n) ? -1.0 : 1.0)

static int
istriangle (const int two_ja, const int two_jb, const int two_jc)
     /* Returns 0 if the triangle condition is met, and the sum of twice the
        angular momenta is even. Arguments are twice the value of the angular
        momenta. */
{
  if ((two_jc <= two_ja + two_jb) && (two_jc >= abs (two_ja - two_jb))
      && (GSL_IS_EVEN (two_ja + two_jb + two_jc)))
    return 1;
  else
    return 0;
}

static int
lndelta_e (const int two_j1, const int two_j2,
           const int two_j3, gsl_sf_result * lndelta)
     /* Calculates the natural log of the delta function - Zare Eq. A-2. Note:
        values returned from gsl_sf_ln_fact_e are always positive. */
{
  gsl_sf_result a1, a2, a3, a4;
  int status;

  status = gsl_sf_lnfact_e ((two_j1 + two_j2 - two_j3) / 2, &a1);
  status += gsl_sf_lnfact_e ((two_j1 - two_j2 + two_j3) / 2, &a2);
  status += gsl_sf_lnfact_e ((-two_j1 + two_j2 + two_j3) / 2, &a3);
  status += gsl_sf_lnfact_e ((two_j1 + two_j2 + two_j3) / 2 + 1, &a4);

  if (status != GSL_SUCCESS)
    OVERFLOW_ERROR (lndelta);

  lndelta->val = 0.5 * (a1.val + a2.val + a3.val - a4.val);
  lndelta->err = 0.5 * (a1.err + a2.err + a3.err + a4.err);
  lndelta->err += 2.0 * GSL_DBL_EPSILON * (a1.val + a2.val + a3.val + a4.val);

  return GSL_SUCCESS;
}

int
gsl_sf_wigner_3j_e (const int two_j1, const int two_j2, const int two_j3,
                    const int two_m1, const int two_m2, const int two_m3,
                    gsl_sf_result * result)
     /* Returns the value of the wigner 3j symbol - Zare Eq. A-1. Note that in
        Zare's book, this equation contains typos. */
{
  int v, vmin, vmax, t1, t2, t3, t4, t5, status;
  gsl_sf_result a, a1, a2, a3, a4, a5, a6, a7, a8;

  if ((two_j1 < 0) || (two_j2 < 0) || (two_j3 < 0) ||
      (abs (two_m1) > two_j1) || (abs (two_m2) > two_j2) ||
      (abs (two_m3) > two_j3) || GSL_IS_ODD (two_j1 + two_m1) ||
      GSL_IS_ODD (two_j2 + two_m2) || GSL_IS_ODD (two_j3 + two_m3))
    DOMAIN_ERROR (result);

  result->val = 0.0;
  result->err = 0.0;

  if ((!istriangle (two_j1, two_j2, two_j3)) ||
      (two_m1 + two_m2 + two_m3) != 0)
    return GSL_SUCCESS;

  t1 = (two_j1 + two_j2 - two_j3) / 2;
  t2 = (two_j1 - two_m1) / 2;
  t3 = (two_j2 + two_m2) / 2;
  t4 = (two_j3 - two_j2 + two_m1) / 2;
  t5 = (two_j3 - two_j1 - two_m2) / 2;

  vmin = (t4 < t5) ? -t4 : -t5;
  vmin = (vmin < 0) ? 0 : vmin;

  vmax = t1;
  vmax = (t2 < vmax) ? t2 : vmax;
  vmax = (t3 < vmax) ? t3 : vmax;

  if (vmin > vmax)
    return GSL_SUCCESS;

  status = gsl_sf_lnfact_e ((two_j1 + two_m1) / 2, &a1);
  status += gsl_sf_lnfact_e (t2, &a2);
  status += gsl_sf_lnfact_e (t3, &a3);
  status += gsl_sf_lnfact_e ((two_j2 - two_m2) / 2, &a4);
  status += gsl_sf_lnfact_e ((two_j3 + two_m3) / 2, &a5);
  status += gsl_sf_lnfact_e ((two_j3 - two_m3) / 2, &a6);
  status += lndelta_e (two_j1, two_j2, two_j3, &a7);

  if (status != GSL_SUCCESS)
    OVERFLOW_ERROR (result);

  a.val =
    0.5 * (a1.val + a2.val + a3.val + a4.val + a5.val + a6.val) + a7.val;
  a.err =
    0.5 * (a1.err + a2.err + a3.err + a4.err + a5.err + a6.err) + a7.err;
  a.err += 2.0 * GSL_DBL_EPSILON * a.val;

  for (v = vmin; v <= vmax; v++)
    {
      status += gsl_sf_lnfact_e (v, &a1);
      status += gsl_sf_lnfact_e (t1 - v, &a2);
      status += gsl_sf_lnfact_e (t2 - v, &a3);
      status += gsl_sf_lnfact_e (t3 - v, &a4);
      status += gsl_sf_lnfact_e (t4 + v, &a5);
      status += gsl_sf_lnfact_e (t5 + v, &a6);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      a7.val = a.val - (a1.val + a2.val + a3.val + a4.val + a5.val + a6.val);
      a7.err = (a.err + a1.err + a2.err + a3.err + a4.err + a5.err + a6.err);
      a7.err += 2.0 * GSL_DBL_EPSILON *
        (a.val + a1.val + a2.val + a3.val + a4.val + a5.val + a6.val);

      status += gsl_sf_exp_err_e (a7.val, a7.err, &a8);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      result->val += PHASE (v) * a8.val;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs (result->val);
    }

  result->val *= PHASE ((two_j1 - two_j2 - two_m3) / 2);
  result->err += 2.0 * GSL_DBL_EPSILON * fabs (result->val);

  return GSL_SUCCESS;
}

int
gsl_sf_wigner_6j_e (const int two_j1, const int two_j2, const int two_j3,
                    const int two_j4, const int two_j5, const int two_j6,
                    gsl_sf_result * result)
     /* Returns the value of the 6j symbol - Zare Eq. A-4 */
{
  int v, vmin, vmax, t1, t2, t3, t4, t5, t6, t7, status;
  gsl_sf_result a, a1, a2, a3, a4;

  if ((two_j1 < 0) || (two_j2 < 0) || (two_j3 < 0) ||
      (two_j4 < 0) || (two_j5 < 0) || (two_j6 < 0))
    DOMAIN_ERROR (result);

  result->val = 0.0;
  result->err = 0.0;

  if (!istriangle (two_j1, two_j2, two_j3) ||
      !istriangle (two_j1, two_j5, two_j6) ||
      !istriangle (two_j4, two_j2, two_j6) ||
      !istriangle (two_j4, two_j5, two_j3))
    return GSL_SUCCESS;

  t1 = (two_j1 + two_j2 + two_j3) / 2;
  t2 = (two_j1 + two_j5 + two_j6) / 2;
  t3 = (two_j4 + two_j2 + two_j6) / 2;
  t4 = (two_j4 + two_j5 + two_j3) / 2;
  t5 = (two_j1 + two_j2 + two_j4 + two_j5) / 2;
  t6 = (two_j2 + two_j3 + two_j5 + two_j6) / 2;
  t7 = (two_j3 + two_j1 + two_j6 + two_j4) / 2;

  vmin = t1;
  vmin = (t2 > vmin) ? t2 : vmin;
  vmin = (t3 > vmin) ? t3 : vmin;
  vmin = (t4 > vmin) ? t4 : vmin;

  vmax = t5;
  vmax = (t6 < vmax) ? t6 : vmax;
  vmax = (t7 < vmax) ? t7 : vmax;

  if (vmin > vmax)
    return GSL_SUCCESS;

  status = lndelta_e (two_j1, two_j2, two_j3, &a1);
  status += lndelta_e (two_j1, two_j5, two_j6, &a2);
  status += lndelta_e (two_j4, two_j2, two_j6, &a3);
  status += lndelta_e (two_j4, two_j5, two_j3, &a4);

  if (status != GSL_SUCCESS)
    OVERFLOW_ERROR (result);

  a.val = a1.val + a2.val + a3.val + a4.val;
  a.err = a1.err + a2.err + a3.err + a4.err;
  a.err += 2.0 * GSL_DBL_EPSILON * a.val;

  for (v = vmin; v <= vmax; v++)
    {
      gsl_sf_result b1, b2, b3, b4, b5, b6, b7, b8, b9, b10;
      status += gsl_sf_lnfact_e (v + 1, &b1);
      status += gsl_sf_lnfact_e (v - t1, &b2);
      status += gsl_sf_lnfact_e (v - t2, &b3);
      status += gsl_sf_lnfact_e (v - t3, &b4);
      status += gsl_sf_lnfact_e (v - t4, &b5);
      status += gsl_sf_lnfact_e (t5 - v, &b6);
      status += gsl_sf_lnfact_e (t6 - v, &b7);
      status += gsl_sf_lnfact_e (t7 - v, &b8);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      b9.val = a.val + b1.val - b2.val - b3.val - b4.val - b5.val - b6.val -
        b7.val - b8.val;
      b9.err = a.err + b1.err + b2.err + b3.err + b4.err + b5.err + b6.err +
        b7.err + b8.err;
      b9.err += 2.0 * GSL_DBL_EPSILON * (a.val + b1.val + b2.val + b3.val +
                                         b4.val + b5.val + b6.val + b7.val +
                                         b8.val);

      status += gsl_sf_exp_err_e (b9.val, b9.err, &b10);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      result->val += b10.val * PHASE (v);
      result->err += b10.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs (result->val);
    }

  return GSL_SUCCESS;
}

int
gsl_sf_wigner_9j_e (const int two_j1, const int two_j2, const int two_j3,
                    const int two_j4, const int two_j5, const int two_j6,
                    const int two_j7, const int two_j8, const int two_j9,
                    gsl_sf_result * result)
    /* Returns the value of 9j symbol - Zare Eq. 4.24 */
{
  int two_k, two_kmin, two_kmax, status = GSL_SUCCESS;

  if ((two_j1 < 0) || (two_j2 < 0) || (two_j3 < 0) ||
      (two_j4 < 0) || (two_j5 < 0) || (two_j6 < 0) ||
      (two_j7 < 0) || (two_j8 < 0) || (two_j9 < 0))
    DOMAIN_ERROR (result);

  result->val = 0.0;
  result->err = 0.0;

  if (!istriangle (two_j1, two_j2, two_j3) ||
      !istriangle (two_j4, two_j5, two_j6) ||
      !istriangle (two_j7, two_j8, two_j9) ||
      !istriangle (two_j1, two_j4, two_j7) ||
      !istriangle (two_j2, two_j5, two_j8) ||
      !istriangle (two_j3, two_j6, two_j9))
    return GSL_SUCCESS;

  two_kmin = abs (two_j1 - two_j9);
  two_kmin =
    (abs (two_j8 - two_j4) < two_kmin) ? abs (two_j8 - two_j4) : two_kmin;
  two_kmin =
    (abs (two_j6 - two_j2) < two_kmin) ? abs (two_j6 - two_j2) : two_kmin;

  two_kmax = (two_j1 + two_j9);
  two_kmax = (two_j8 + two_j4 > two_kmax) ? two_j8 + two_j4 : two_kmax;
  two_kmax = (two_j6 + two_j2 > two_kmax) ? two_j6 + two_j2 : two_kmax;

  if (two_kmax < two_kmin)
    {
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }

  for (two_k = two_kmin; two_k <= two_kmax; two_k += 2)
    {
      gsl_sf_result a1, a2, a3;
      status += gsl_sf_wigner_6j_e (two_j1, two_j4, two_j7,
                                    two_j8, two_j9, two_k, &a1);
      status +=
        gsl_sf_wigner_6j_e (two_j2, two_j5, two_j8,
                            two_j4, two_k, two_j6, &a2);
      status +=
        gsl_sf_wigner_6j_e (two_j3, two_j6, two_j9,
                            two_k, two_j1, two_j2, &a3);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      result->val += (two_k + 1.0) * PHASE (two_k) * a1.val * a2.val * a3.val;

      result->err += result->val * (fabs (a1.err / a1.val) +
                                    fabs (a2.err / a2.val) +
                                    fabs (a3.err / a3.val));
      result->err += 2.0 * GSL_DBL_EPSILON * fabs (result->val);
    }
  return GSL_SUCCESS;
}

int
gsl_sf_wigner_drot_e (const int two_j, const int two_m1, const int two_m2,
                      const double theta, gsl_sf_result * result)
     /* Returns the value of the reduced rotation matrix - Zare Eq. 3.57. (which
        contains typos).  Angle theta is in radians. */
{
  int v, vmin, vmax, t1, t2, t3, t4, tv, status = GSL_SUCCESS;
  gsl_sf_result a, a1, a2, a3, a4, st, ct;

  if ((two_j) < 0 || abs (two_m1) > two_j || abs (two_m2) > two_j ||
      (GSL_IS_ODD (two_j + two_m1)) || (GSL_IS_ODD (two_j + two_m2)))
    DOMAIN_ERROR (result);

  result->val = 0.0;
  result->err = 0.0;

  t1 = (two_j + two_m1) / 2;
  t2 = (two_j - two_m2) / 2;
  t3 = (two_m2 - two_m1) / 2;
  t4 = (2 * two_j + two_m1 - two_m2) / 2;

  vmin = (-t3 < 0) ? 0 : -t3;
  vmax = (t2 < t1) ? t2 : t1;

  if (vmin > vmax)
    return GSL_SUCCESS;

  status += gsl_sf_cos_e (theta * 0.5, &ct);
  status += gsl_sf_sin_e (theta * 0.5, &st);
  if (status != GSL_SUCCESS)
    OVERFLOW_ERROR (result);

  status += gsl_sf_lnfact_e (t1, &a1);
  status += gsl_sf_lnfact_e (t2, &a2);
  status += gsl_sf_lnfact_e ((two_j - two_m1) / 2, &a3);
  status += gsl_sf_lnfact_e ((two_j + two_m2) / 2, &a4);
  if (status != GSL_SUCCESS)
    OVERFLOW_ERROR (result);

  a.val = 0.5 * (a1.val + a2.val + a3.val + a4.val);
  a.err = 0.5 * (a1.err + a2.err + a3.err + a4.err);
  a.err += 2.0 * GSL_DBL_EPSILON * a.val;

  for (v = vmin; v <= vmax; v++)
    {
      gsl_sf_result b1, b2, b3, b4, b5, b6, b7, b8;
      int i1, i2;

      status += gsl_sf_lnfact_e (t1 - v, &b1);
      status += gsl_sf_lnfact_e (t2 - v, &b2);
      status += gsl_sf_lnfact_e (t3 + v, &b3);
      status += gsl_sf_lnfact_e (v, &b4);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      b5.val = a.val - b1.val - b2.val - b3.val - b4.val;
      b5.err = a.err + b1.err + b2.err + b3.err + b4.err;
      b5.err +=
        2.0 * GSL_DBL_EPSILON * (a.val + b1.val + b2.val + b3.val + b4.val);

      status += gsl_sf_exp_err_e (b5.val, b5.err, &b6);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      tv = 2 * v;
      i1 = t4 - tv;
      i2 = t3 + tv;

      status += gsl_sf_pow_int_e (ct.val, i1, &b7);
      status += gsl_sf_pow_int_e (st.val, i2, &b8);
      if (status != GSL_SUCCESS)
        OVERFLOW_ERROR (result);

      /* Bolt on error in pow_int for the error in the input values */
      b7.err += i1 * fabs (ct.err);
      b8.err += i2 * fabs (st.err);

      result->val += PHASE (v) * b6.val * b7.val * b8.val;

      result->err += fabs (result->val) *
        (fabs (b6.err / b6.val) + fabs (b7.err / b7.val) +
         fabs (b8.err / b8.val));
      result->err += 2.0 * GSL_DBL_EPSILON * fabs (result->val);
    }

  return GSL_SUCCESS;
}

#include "eval.h"

double
gsl_sf_wigner_3j (const int two_j1, const int two_j2, const int two_j3,
                  const int two_m1, const int two_m2, const int two_m3)
{
  EVAL_RESULT (gsl_sf_wigner_3j_e (two_j1, two_j2, two_j3,
                                   two_m1, two_m2, two_m3, &result));
}

double
gsl_sf_wigner_6j (const int two_j1, const int two_j2, const int two_j3,
                  const int two_j4, const int two_j5, const int two_j6)
{
  EVAL_RESULT (gsl_sf_wigner_6j_e (two_j1, two_j2, two_j3,
                                   two_j4, two_j5, two_j6, &result));
}

double
gsl_sf_wigner_9j (const int two_j1, const int two_j2, const int two_j3,
                  const int two_j4, const int two_j5, const int two_j6,
                  const int two_j7, const int two_j8, const int two_j9)
{
  EVAL_RESULT (gsl_sf_wigner_9j_e (two_j1, two_j2, two_j3,
                                   two_j4, two_j5, two_j6,
                                   two_j7, two_j8, two_j9, &result));
}

double
gsl_sf_wigner_drot (const int two_j, const int two_m1, const int two_m2,
                    const double theta)
{
  EVAL_RESULT (gsl_sf_wigner_drot_e (two_j, two_m1, two_m2, theta, &result));
}

#undef PHASE


/*
    This code is written by <albanese@fbk.it>.
    (C) 2010 mlpy Developers.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#define MIN( A , B ) ((A) < (B) ? (A) : (B))

#define INIT_STD 0
#define INIT_PLUSPLUS 1

void init_std(double *data,      /* data points (nn points x pp dimensions) */
	      double *means,     /* means (kk clusters x pp dimensions) */
	      int nn,            /* number od data points */
	      int pp,            /* number of dimensions */
	      int kk,            /* number of clusters */
	      unsigned long seed /* random seed for init */
	      )
{
  int n, p, k;
  int *ridx;
  const gsl_rng_type * T;
  gsl_rng * r;

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);
  gsl_rng_set (r, seed);

  ridx = (int *) malloc (nn * sizeof(int));

  for (n=0; n<nn; n++)
    ridx[n] = n;

  gsl_ran_shuffle (r, ridx, nn, sizeof (int));

  for (k=0; k<kk; k++)
    for (p=0; p<pp; p++)
      means[p + (k * pp)] = data[p + (ridx[k] * pp)];

  free(ridx);
}

/* for init_plus */
void
dist_min(double *a, double *b, int nn)
{
  int n;

  for (n=0; n<nn; n++)
    a[n] = MIN (a[n], b[n]);
}

/* for init_plus */
int
idx_max(double *a, int nn)
{
  int n, idx = 0;
  double max = -DBL_MAX;

  for (n=0; n<nn; n++)
    if (a[n] > max)
      {
	max = a[n];
	idx = n;
      }

  return idx;
}

void
init_plus(double *data,      /* data points (nn points x pp dimensions) */
	  double *means,     /* means (kk clusters x pp dimensions) */
	  int nn,            /* number od data points */
	  int pp,            /* number of dimensions */
	  int kk,            /* number of clusters */
	  unsigned long seed /* random seed for init */
	  )
{
  int n, p, k;
  double *dist, *distk;
  int sidx;

  const gsl_rng_type *T;
  gsl_rng *r;

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);
  gsl_rng_set (r, seed);

  dist = (double *) malloc (nn * sizeof(double));
  distk = (double *) malloc (nn * sizeof(double));

  /* first mean (randomly selected) */
  sidx = (int) gsl_rng_uniform_int (r, nn);
  gsl_rng_free(r);
  for (p=0; p<pp; p++)
    means[p] = data[p + (sidx * pp)];

  /* initialize dist */
  for (n=0; n<nn; n++) /* for each data point */
    dist[n] = DBL_MAX;

  for (k=0; k<kk-1; k++)
    {
      /* for each data point x compute distance from mean k */
      for (n=0; n<nn; n++)
	{
	  distk[n] = 0.0;
	  for (p=0; p<pp; p++)
	    distk[n] += pow(data[p + (n * pp)] - means[p + (k * pp)], 2);
	}

      /* for each data point x, compute the distance between x and */
      /* the nearest center that has already been chosen */
      dist_min (dist, distk, nn);

      /* add one new data point as a new center, using */
      sidx = idx_max (dist, nn);

      for (p=0; p<pp; p++)
	means[p + ((k+1) * pp)] = data[p + (sidx * pp)];
    }

  free(dist);
  free(distk);
}

/* assignment step */
int
a_step(double *data,  /* data points (nn x pp) */
       double *means, /* means (kk x pp) */
       int *cls,      /* cluster assignement for each data point (nn) */
       int *nelems,   /* number of elements of each cluster (kk) */
       int nn,        /* number od data points */
       int pp,        /* number of dimensions */
       int kk         /* number of clusters */
       )
{
  int n, p, k, kn = 0;
  double dist, dmin;
  int changed = 0;

  for (k=0; k<kk; k++)
    nelems[k] = 0;

  for (n=0; n<nn; n++) /* for each data point */
    {
      dmin = DBL_MAX;
      for (k=0; k<kk; k++) /* for each cluster */
	{
	  /* compute distance */
	  dist = 0.0;
	  for (p=0; p<pp; p++)
	    dist += pow(data[p + (n * pp)] - means[p + (k * pp)], 2);

	  /* remember the cluster k if dist < dmin */
	  if (dist < dmin)
	    {
	      dmin = dist;
	      kn = k;
	    }
	}

      /* if the cluster assignement change */
      if (kn != cls[n])
	changed++;

      /* update clusters and number of elements of each cluster */
      cls[n] = kn;
      nelems[kn]++;
    }

  return changed;
}

/* update step */
int
u_step(double *data,  /* data points (nn x pp) */
       double *means, /* means (kk x pp) */
       int *cls,      /* cluster assignement for each data point (nn) */
       int *nelems,   /* number of elements of each cluster (kk) */
       int nn,        /* number od data points */
       int pp,        /* number of dimensions */
       int kk         /* number of clusters */
       )
{
  int n, p, k;

  /* reset means */
  for (k=0; k<kk; k++)
    for (p=0; p<pp; p++)
      means[p + (k * pp)] = 0.0;

  for (n=0; n<nn; n++) /* for each data point */
    for (p=0; p<pp; p++) /* for each dimension */
      means[p + (cls[n] * pp)] += data[p + (n * pp)];

  for (k=0; k<kk; k++)
    if (nelems[k] > 0)
      for (p=0; p<pp; p++)
	means[p + (k * pp)] /= nelems[k];

  return 1;
}

int
km(double *data,  /* data points (nn x pp) */
   double *means, /* initialized means (kk x pp) */
   int *cls,      /* cluster assignement for each data point (nn) */
   int nn,        /* number od data points */
   int pp,        /* number of dimensions */
   int kk         /* number of clusters */
   )
{
  int n;
  int ret, steps = 0;
  int changed = -1;
  int *nelems; /* number of elements of each cluster (kk) */

  nelems = (int *) malloc (kk * sizeof(int));

  /* init cls */
  for (n=0; n<nn; n++)
    cls[n] = -1.0;

  /* k-means algorithm  */
  while (changed != 0)
    {
      changed = a_step (data, means, cls, nelems, nn, pp, kk);
      ret = u_step (data, means, cls, nelems, nn, pp, kk);
      steps++;
    }

  free(nelems);

  return steps;
}


//CELL operation: does output side of Jordan RNN layer.
//The inputs here are from the IN stage, so reduced to N driving input time-series in X.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int jordan_s (float *Y, const float *X, const float *U, float *Y1, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int jordan_d (double *Y, const double *X, const double *U, double *Y1, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int jordan_s (float *Y, const float *X, const float *U, float *Y1, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;

    float *H;
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in jordan_s: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        H[0] = 1.0f / (1.0f+expf(-X[0]-U[0]*Y1[0]));
        Y[0] = 1.0f / (1.0f+expf(-B[0]-W[0]*H[0]));
        for (size_t t=1; t<T; ++t)
        {
            H[0] = 1.0f / (1.0f+expf(-X[t]-U[0]*Y[t-1]));
            Y[t] = 1.0f / (1.0f+expf(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t n=0u; n<N; ++n) { cblas_scopy((int)T,&B[n],0,&Y[n],(int)N); }
                cblas_scopy((int)N,&X[0],1,H,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],1);
                for (size_t n=0u; n<N; ++n) { Y[n] = 1.0f/(1.0f+expf(-Y[n])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,H,1);
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,&Y[(t-1)*N],1,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t*N],1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-Y[t*N+n])); }
                }
            }
            else
            {
                for (size_t n=0u; n<N; ++n) { cblas_scopy((int)T,&B[n],0,&Y[n*T],1); }
                cblas_scopy((int)N,&X[0],(int)T,H,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],(int)T);
                for (size_t n=0u; n<N; ++n) { Y[n*T] = 1.0f/(1.0f+expf(-Y[n*T])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,H,1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,&Y[t-1],(int)T,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t],(int)T);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-Y[t+n*T])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t n=0u; n<N; ++n) { cblas_scopy((int)T,&B[n],0,&Y[n*T],1); }
                cblas_scopy((int)N,&X[0],(int)T,H,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],(int)T);
                for (size_t n=0u; n<N; ++n) { Y[n*T] = 1.0f/(1.0f+expf(-Y[n*T])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,H,1);
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,&Y[t-1],(int)T,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t],(int)T);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-Y[t+n*T])); }
                }
            }
            else
            {
                for (size_t n=0u; n<N; ++n) { cblas_scopy((int)T,&B[n],0,&Y[n],(int)N); }
                cblas_scopy((int)N,&X[0],1,H,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],1);
                for (size_t n=0u; n<N; ++n) { Y[n] = 1.0f/(1.0f+expf(-Y[n])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,H,1);
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,&Y[(t-1)*N],1,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-H[n])); }
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t*N],1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-Y[t*N+n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in jordan_s: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

int jordan_d (double *Y, const double *X, const double *U, double *Y1, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;

    double *H;
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in jordan_d: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        H[0] = 1.0 / (1.0+exp(-X[0]-U[0]*Y1[0]));
        Y[0] = 1.0 / (1.0+exp(-B[0]-W[0]*H[0]));
        for (size_t t=1; t<T; ++t)
        {
            H[0] = 1.0 / (1.0+exp(-X[t]-U[0]*Y[t-1]));
            Y[t] = 1.0 / (1.0+exp(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t n=0u; n<N; ++n) { cblas_dcopy((int)T,&B[n],0,&Y[n],(int)N); }
                cblas_dcopy((int)N,&X[0],1,H,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],1);
                for (size_t n=0u; n<N; ++n) { Y[n] = 1.0/(1.0+exp(-Y[n])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,H,1);
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,&Y[(t-1)*N],1,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t*N],1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-Y[t*N+n])); }
                }
            }
            else
            {
                for (size_t n=0u; n<N; ++n) { cblas_dcopy((int)T,&B[n],0,&Y[n*T],1); }
                cblas_dcopy((int)N,&X[0],(int)T,H,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],(int)T);
                for (size_t n=0u; n<N; ++n) { Y[n*T] = 1.0/(1.0+exp(-Y[n*T])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,H,1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,&Y[t-1],(int)T,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t],(int)T);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-Y[t+n*T])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t n=0u; n<N; ++n) { cblas_dcopy((int)T,&B[n],0,&Y[n*T],1); }
                cblas_dcopy((int)N,&X[0],(int)T,H,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],(int)T);
                for (size_t n=0u; n<N; ++n) { Y[n*T] = 1.0/(1.0+exp(-Y[n*T])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,H,1);
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,&Y[t-1],(int)T,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t],(int)T);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-Y[t+n*T])); }
                }
            }
            else
            {
                for (size_t n=0u; n<N; ++n) { cblas_dcopy((int)T,&B[n],0,&Y[n],(int)N); }
                cblas_dcopy((int)N,&X[0],1,H,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,Y1,1,o,H,1);
                for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[0],1);
                for (size_t n=0u; n<N; ++n) { Y[n] = 1.0/(1.0+exp(-Y[n])); }
                for (size_t t=1; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,H,1);
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,&Y[(t-1)*N],1,o,H,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-H[n])); }
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,&Y[t*N],1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-Y[t*N+n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in jordan_d: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/*
Copyright (C) 2019-2020 JingWeiZhangHuai <jingweizhanghuai@163.com>
Licensed under the Apache License, Version 2.0; you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

#include <cblas.h>
#include "morn_tensor.h"

void DeconvTensorToMatData(MTensor *tns,int bc,float *mdata,int knl_height,int knl_width,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width*x_stride;
    int out_height=height*y_stride;
    int mwidth = knl_height*knl_width*channel+1;
    int mheight= out_height*out_width;

    memset(mdata,0,mwidth*mheight*sizeof(float));

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int i,j,c;
    for(j=0;j<mheight;j++)
    {
        int n=j/out_width-knl_height/2;
        int m=j%out_width-knl_width/2;
        for(i=0;i<mwidth-1;i+=channel)
        {
            int h= i/(knl_width*channel)         +n;if(h%y_stride!=0) continue;h=h/y_stride;
            int w=(i%(knl_width*channel))/channel+m;if(w%x_stride!=0) continue;w=w/x_stride;

            if(h<0)h=0;else if(h>=height)h=height-1;
            if(w<0)w=0;else if(w>= width)w= width-1;

            for(c=0;c<channel;c++)
                mdata[(j*mwidth)+i+c]=tdata[c*tsize+h*width+w];
        }
        mdata[(j*mwidth)+mwidth-1]=1.0f;
    }
}

void DeconvMatDataToTensor(float *mdata,MTensor *tns,int bc,int knl_height,int knl_width,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width*x_stride;
    int out_height=height*y_stride;

    int mwidth = knl_height*knl_width*channel+1;
    int mheight= out_height*out_width;

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int i,j,c;
    for(j=0;j<mheight;j++)
    {
        int n=j/out_width-knl_height/2;
        int m=j%out_width-knl_width/2;
        for(i=0;i<mwidth-1;i+=channel)
        {
            int h= i/(knl_width*channel)         +n;if(h%y_stride!=0) continue;h=h/y_stride;
            int w=(i%(knl_width*channel))/channel+m;if(w%x_stride!=0) continue;w=w/x_stride;

            if(h<0)h=0;else if(h>=height)h=height-1;
            if(w<0)w=0;else if(w>= width)w= width-1;

            for(c=0;c<channel;c++)
                tdata[c*tsize+h*width+w]+=mdata[(j*mwidth)+i+c];
        }
        mdata[(j*mwidth)+mwidth-1]=1.0f;
    }
}

struct TensorDeconvPara
{
    MLayer *prev;

    int knl_num;
    int knl_height;
    int knl_width;

    int x_stride;
    int y_stride;

    int res_valid;

    float rate;
    float decay;
    float momentum;
};
void *mTensorDeconvPara(MFile *ini,char *name)
{
    struct TensorDeconvPara *para = (struct TensorDeconvPara *)mMalloc(sizeof(struct TensorDeconvPara));

    char *value = mINIRead(ini,name,"prev");
    para->prev = mNetworkLayer(ini,value);
    mException((para->prev == NULL),EXIT,"invalid prev");

    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    value = mINIRead(ini,name,"knl_num");
    if(value != NULL) para->knl_num= atoi(value);else para->knl_num= 1;

    value = mINIRead(ini,name,"knl_height");
    if(value != NULL) para->knl_height= atoi(value);else para->knl_height= 1;

    value = mINIRead(ini,name,"knl_width");
    if(value != NULL) para->knl_width= atoi(value);else para->knl_width= 1;

    value = mINIRead(ini,name,"x_stride");
    if(value != NULL) para->x_stride= atoi(value);else para->x_stride= 1;

    value = mINIRead(ini,name,"y_stride");
    if(value != NULL) para->y_stride= atoi(value);else para->y_stride= 1;

    value = mINIRead(ini,name,"rate");
    if(value != NULL) para->rate = atof(value);
    else
    {
        value = mINIRead(ini,"para","rate");
        if(value != NULL) para->rate = atof(value);
        else              para->rate = 0.001;
    }

    value = mINIRead(ini,name,"decay");
    if(value != NULL) para->decay = atof(value);
    else
    {
        value = mINIRead(ini,"para","decay");
        if(value != NULL) para->decay = atof(value);
        else              para->decay = 0.01;
    }
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    value = mINIRead(ini,name,"momentum");
    if(value != NULL) para->momentum = atof(value);
    else
    {
        value = mINIRead(ini,"para","momentum");
        if(value != NULL) para->momentum = atof(value);
        else              para->momentum = 0.9;
    }

    return para;
}

struct HandleTensorDeconv
{
    float *mat;
    float *kernel;
    float *update;
};
void endTensorDeconv(void *info)
{
    struct HandleTensorDeconv *handle = (struct HandleTensorDeconv *)info;
    if(handle->mat   != NULL) mFree(handle->mat);
    if(handle->kernel!= NULL) mFree(handle->kernel);
    if(handle->update!= NULL) mFree(handle->update);
}
#define HASH_TensorDeconv 0x9087d39c
void TensorDeconvSet(MLayer *layer)
{
    if(layer->state != DFLT) return;
    struct TensorDeconvPara *para = (struct TensorDeconvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->tns;

    MHandle *hdl=mHandle(out,TensorDeconv);
    struct HandleTensorDeconv *handle = (struct HandleTensorDeconv *)(hdl->handle);

    int out_height= in->height*para->y_stride;
    int out_width = in->width *para->x_stride;
    int mheight = (out_height*out_width);
    int mwidth = para->knl_height*para->knl_width*in->channel+1;
    int data_size = para->knl_num*mwidth;

    mTensorRedefine(out,in->batch,para->knl_num,out_height,out_width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        if(INVALID_TENSOR(res)) mTensorRedefine(res,in->batch,in->channel,in->height,in->width,in->data);
        else                    mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);

        if(handle->update != NULL) mFree(handle->update);
        handle->update =(float *)mMalloc(data_size*sizeof(float));
        memset(handle->update,0,data_size*sizeof(float));
    }

    if(handle->kernel !=NULL) mFree(handle->kernel);
    handle->kernel = (float *)mMalloc(data_size*sizeof(float));

    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/mwidth);
        for(int i=0;i<data_size;i++)
            handle->kernel[i] = scale*mNormalRand(0.0f,1.0f);
    }
    else
        mNetworkParaRead(layer,"kernel",handle->kernel,data_size*sizeof(float));

    if(handle->mat!=NULL) mFree(handle->mat);
    handle->mat = (float *)mMalloc(mheight*mwidth*sizeof(float));

    hdl->valid = 1;
}

void mTensorDeconvForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    mException(strcmp("Deconv",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorDeconvPara *para = (struct TensorDeconvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *out=layer->tns;

    TensorDeconvSet(layer);

    MHandle *hdl=mHandle(out,TensorDeconv);
    struct HandleTensorDeconv *handle = (struct HandleTensorDeconv *)(hdl->handle);

    int mheight = (out->height*out->width);
    int mwidth = para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *in_data = handle->mat;

    for(int b=0;b<in->batch;b++)
    {
        DeconvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasTrans,
                    para->knl_num,mheight,mwidth,
                    1.0f,
                    kernel_data,mwidth,
                        in_data,mwidth,
               0.0f,   out_data,mheight);
    }

    layer->state = MORN_FORWARD;
}

void mTensorDeconvBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    mException(strcmp("Deconv",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorDeconvPara *para = (struct TensorDeconvPara *)(layer->para);

    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->res;

    MHandle *hdl=mHandle(layer->tns,TensorDeconv);
    struct HandleTensorDeconv *handle = (struct HandleTensorDeconv *)(hdl->handle);
    mException((hdl->valid == 0),EXIT,"no forward operate");

    int mheight = (out->height*out->width);
    int mwidth = para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *update_data= handle->update;
    float *    in_data= handle->mat;
    float *   res_data= handle->mat;

    mNetworkParaWrite(layer,"kernel",kernel_data,para->knl_num*mwidth*sizeof(float));

    for(int b=0;b<in->batch;b++)
    {
        DeconvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,
                    para->knl_num,mwidth,mheight,
                    1.0f,
                       out_data,mheight,
                        in_data,mwidth,
                    (b==0)?para->momentum:1.0f,
                    update_data,mwidth);
    }
    cblas_saxpby(para->knl_num*mwidth,
                 (0.0f-(para->rate/(float)(in->batch))),update_data,1,
                 (1.0f-(para->decay*para->rate))       ,kernel_data,1);

    if(para->res_valid==0) return;

    if(para->prev->state == MORN_FORWARD)
    {
        for(int b=0;b<res->batch;b++)
            memset(res->data[b],0,in->height*in->width*in->channel*sizeof(float));
        para->prev->state = MORN_BACKWARD;
    }

    for(int b=0;b<in->batch;b++)
    {
        float *out_data = out->data[b];

        cblas_sgemm(CblasRowMajor,CblasTrans,CblasNoTrans,
                    mheight,mwidth,para->knl_num,
                    1.0f,
                       out_data,mheight,
                    kernel_data,mwidth,
                0.0,   res_data,mwidth);

        DeconvMatDataToTensor(res_data,res,b,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
    }
}


//
//  emu.c
//  Created by Earl Lawrence on 11/10/16.
//  Modified E Chisari 05/10/17 for CCL
//  For details on the license, see ../LICENSE_COSMICEMU
//  in this repository.
//

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

#include "ccl.h"

#include "ccl_emu17_params.h"
#include "ccl_emu17.h"

// Sizes of stuff
static int m[2] = {111, 36}, neta=2808, peta[2]={7, 28}, rs=8, p=8;

// Kriging basis computed by emuInit
// Sizes of each basis will be peta[ee] and m[ee]
static double KrigBasis[2][28][111];

// Need to combine some things into a big thing.
static double beta[2][28][8];
static double w[2][28][111];
static double lamws[2][28];
static double lamz[2][28];

// Initialization to compute the kriging basis for the two parts
static void emuInit() {

    int ee, i, j, k, l;
    double cov;
    gsl_matrix *SigmaSim;
    gsl_vector *b;

    // Because of the structure of this emu, I need to do this horrible part first.
    // Fill in the correlation lenghths
    for(i=0; i<7; i++) {
        for(j=0; j<8; j++) {
            beta[0][i][j] = beta1[i][j];
        }
    }

    for(i=0; i<28; i++) {
        for(j=0; j<8; j++) {
            beta[1][i][j] = beta2[i][j];
        }
    }

    // Fill in the PC weights
    for(i=0; i<7; i++) {
        for(j=0; j<111; j++) {
            w[0][i][j] = w1[i][j];
        }
    }

    for(i=0; i<28; i++) {
        for(j=0; j<36; j++) {
            w[1][i][j] = w2[i][j];
        }
    }

    // Fill in the precisions
    for(i=0; i<7; i++) {
        lamws[0][i] = lamws1[i];
        lamz[0][i] = lamz1[i];
    }

    for(i=0; i<28; i++) {
        lamws[1][i] = lamws2[i];
        lamz[1][i] = lamz2[i];
    }

    // This emu has two parts: one that uses all m[0] of the data for the the first peta[0] components
    // and another that uses only the m[1] complete data for the next peta[1] components.
    for(ee=0; ee<2; ee++) {

        // Allocate some stuff
        SigmaSim = gsl_matrix_alloc(m[ee], m[ee]);
        b = gsl_vector_alloc(m[ee]);

        // Loop over the basis
        for(i=0; i<peta[ee]; i++) {

            // Loop over the number of simulations
            for(j=0; j<m[ee]; j++) {

                // Diagonal term
                gsl_matrix_set(SigmaSim, j, j, (1.0/lamz[ee][i]) + (1.0/lamws[ee][i]));

                // Off-diagonals
                for(k=0; k<j; k++) {

                    // compute the covariance
                    cov = 0.0;
                    for(l=0; l<p; l++) {
                        cov -= beta[ee][i][l]*pow(x[j][l] - x[k][l], 2.0);
                    } // for(l=0; l<p; l++)
                    cov = exp(cov) / lamz[ee][i];

                    // put the covariance where it belongs
                    gsl_matrix_set(SigmaSim, j, k, cov);
                    gsl_matrix_set(SigmaSim, k, j, cov);

                } // for(k=0; k<j; k++)

                // Vector for the PC weights
                gsl_vector_set(b, j, w[ee][i][j]);

            } // for(j=0; j<m[ee]; j++)

            // Cholesky and solve
            gsl_linalg_cholesky_decomp(SigmaSim);
            gsl_linalg_cholesky_svx(SigmaSim, b);

            // Put b where it belongs in the Kriging basis
            for(j=0; j<m[ee]; j++) {
                KrigBasis[ee][i][j] = gsl_vector_get(b, j);
            }

        } // for(i=0; i<peta[ee]; i++)

        // Clean this up
        gsl_matrix_free(SigmaSim);
        gsl_vector_free(b);

    } // for(ee=0; ee<2; ee+)

} // emuInit()

// Actual emulation
void ccl_pkemu(double *xstar, double **ystar, int* status, ccl_cosmology* cosmo) {

    static double inited=0;
    int ee, i, j, k;
    double wstar[peta[0]+peta[1]];
    double Sigmastar[2][peta[1]][m[0]];
    double ystaremu[neta];
    *ystar=(double *)malloc(sizeof(double)*NK_EMU);
    double ybyz[rs];
    double logc;
    double xstarstd[p];
    int zmatch;
    gsl_spline *zinterp = gsl_spline_alloc(gsl_interp_cspline, rs);
    gsl_interp_accel *accel = gsl_interp_accel_alloc();

    // Initialize if necessary
    if(inited==0) {
        emuInit();
        inited = 1;
    }

    // Transform w_a into (-w_0-w_a)^(1/4)
    xstar[6] = pow(-xstar[5]-xstar[6], 0.25);
    // Check the inputs to make sure we're interpolating.
    for(i=0; i<p; i++) {
        if((xstar[i] < xmin[i]) || (xstar[i] > xmax[i])) {
            switch(i) {
                case 0:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): omega_m must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 1:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): omega_b must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 2:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): sigma8 must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 3:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): h must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 4:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): n_s must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 5:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): w_0 must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 6:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): (-w_0-w_a)^(1/4) must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
                case 7:
                    ccl_cosmology_set_status_message(cosmo,
                            "ccl_pkemu(): omega_nu must be between %f and %f.\n",
                            xmin[i], xmax[i]);
                    break;
            }
            *status = CCL_ERROR_EMULATOR_BOUND;
            ccl_raise_exception(*status, cosmo->status_message);
	    gsl_spline_free(zinterp);
	    gsl_interp_accel_free(accel);
            return;
        }
    } // for(i=0; i<p; i++)
    if((xstar[p] < z[0]) || (xstar[p] > z[rs-1])) {
        ccl_cosmology_set_status_message(cosmo,
                "ccl_pkemu(): z must be between %f and %f.\n",
                z[0], z[rs-1]);
        *status = CCL_ERROR_EMULATOR_BOUND;
        ccl_raise_exception(*status, cosmo->status_message);
	gsl_spline_free(zinterp);
	gsl_interp_accel_free(accel);
        return;
    }

    // Standardize the inputs
    for(i=0; i<p; i++) {
        xstarstd[i] = (xstar[i] - xmin[i]) / xrange[i];
    }

    // compute the covariances between the new input and sims for all the PCs.
    for(ee=0; ee<2; ee++) {
        for(i=0; i<peta[ee]; i++) {
            for(j=0; j<m[ee]; j++) {
                logc = 0.0;
                for(k=0; k<p; k++) {
                    logc -= beta[ee][i][k]*pow(x[j][k] - xstarstd[k], 2.0);
                }
                Sigmastar[ee][i][j] = exp(logc) / lamz[ee][i];
            }
        }
    }

    // Compute wstar for the first chunk.
    for(i=0; i<peta[0]; i++) {
        wstar[i]=0.0;
        for(j=0; j<m[0]; j++) {
            wstar[i] += Sigmastar[0][i][j] * KrigBasis[0][i][j];
        }
    }
    // Compute wstar for the second chunk.
    for(i=0; i<peta[1]; i++) {
        wstar[peta[0]+i]=0.0;
        for(j=0; j<m[1]; j++) {
            wstar[peta[0]+i] += Sigmastar[1][i][j] * KrigBasis[1][i][j];
        }
    }

    /*
    for(i=0; i<peta[0]+peta[1]; i++) {
        printf("%f\n", wstar[i]);
    }
     */

    // Compute ystar, the new output
    for(i=0; i<neta; i++) {
        ystaremu[i] = 0.0;
        for(j=0; j<peta[0]+peta[1]; j++) {
            ystaremu[i] += K[i][j]*wstar[j];
        }
        ystaremu[i] = ystaremu[i]*sd + mean[i];

        // Convert to P(k)
        //ystaremu[i] = ystaremu[i] - 1.5*log10(mode[i % NK_EMU]);
        //ystaremu[i] = 2*M_PI*M_PI*pow(10, ystaremu[i]);
    }

    // Interpolate to the desired redshift
    // Natural cubic spline interpolation over z.

    // First check to see if the requested z is one of the training z.
    zmatch = -1;
    for(i=0; i<rs; i++) {
        if(xstar[p] == z[i]) {
            zmatch = rs-i-1;
        }
    }

    // z doesn't match a training z, interpolate
    if(zmatch == -1) {
        for(i=0; i<NK_EMU; i++) {
            for(j=0; j<rs; j++) {
                ybyz[rs-j-1] = ystaremu[j*NK_EMU+i];
            }
            gsl_spline_init(zinterp, z, ybyz, rs);
            (*ystar)[i] = gsl_spline_eval(zinterp, xstar[p], accel);
            gsl_interp_accel_reset(accel);
        }

    } else { //otherwise, copy in the emulated z without interpolating
        for(i=0; i<NK_EMU; i++) {
	  (*ystar)[i] = ystaremu[zmatch*NK_EMU + i];
        }
    }

    gsl_spline_free(zinterp);
    gsl_interp_accel_free(accel);

    // Convert to P(k)
    for(i=0; i<NK_EMU; i++) {
      (*ystar)[i] = (*ystar)[i] - 1.5*log10(mode[i]) + log10(2) + 2*log10(M_PI);
      (*ystar)[i] = pow(10, (*ystar)[i]);
    }
}


#include "./economic-model.h"
#include <gsl/gsl_randist.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

person *makePerson(int birthYear, int deathYear, int iq,
                   personality *_personality)
{
  person *p = malloc(sizeof(person));
  p->birth_year = birthYear;
  p->death_year = deathYear;
  p->iq = iq;
  p->personality = _personality;
  return p;
}

int getAge(int year, person *p)
{
  if (p == NULL)
  {
    return -1;
  }

  if (p->death_year > -1)
  {
    return (p->death_year - p->birth_year);
  }
  return year - p->birth_year;
}

int healthCheck(int year, person *p)
{
  if (p->death_year != -1)
  {
    return p->death_year;
  }

  int age = getAge(year, p);

  if (age >= 100)
  {
    p->death_year = year;
    return year;
  }

  return -1;
}

void personTick(int year, person *p) { healthCheck(year, p); }

int countPeople(person **people)
{
  int i = 0;
  while (people[i] != NULL)
  {
    i++;
  }
  return i;
}

/**
 * TODO: growth should be logarithmic - easy to grow early when
 * complexity is low and harder thereafter.
 *
 * Growth may even be negative e.g. starting a high level of experience over
 * the past years but this year the intensity drops. Yearly attrition.
 */
int getGrowthFromExperience(experience_option *option, person *p)
{
  double r = getRandomGaussian(5, 2, NULL, NULL);
  return round(r);
}


#ifndef OPT_H
#define OPT_H
#include <gsl/gsl_vector.h>

void optimize_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, gsl_vector* opt_x);
double f_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, const gsl_vector* opt_x);
void df_simplex(const gsl_vector* gamma, const gsl_vector* v, double lambda, const gsl_vector* opt_x, gsl_vector* g);
bool is_feasible(const gsl_vector* x);
void simplex_projection(const gsl_vector* x, gsl_vector* x_proj, double z=1.0);

#endif // OPT_H


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions computing coeffcients of Not-A-Knot and Quadratic splines in matrix form.
 *
 */

#ifndef _SPLINECOEFFS_H
#define _SPLINECOEFFS_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"

#if defined(__cplusplus)
extern "C" {
#elif 0
} /* so that editors will match preceding brace */
#endif

void BuildNotAKnotSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n);                      /* Size of x, y, and of output matrix */

void BuildQuadSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n);                      /* Size of x, y, and of output matrix */

void BuildSplineCoeffs(
  CAmpPhaseSpline** splines,                  /*  */
  CAmpPhaseFrequencySeries* freqseries);      /*  */

void BuildListmodesCAmpPhaseSpline(
  ListmodesCAmpPhaseSpline** listspline,              /* Output: list of modes of splines in matrix form */
  ListmodesCAmpPhaseFrequencySeries* listh);          /* Input: list of modes in amplitude/phase form */

/* Functions for spline evaluation */

/* Note: for the spines in matrix form, the first column contains the x values, so the coeffs start at 1 */
double EvalCubic(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2,         /**/
  double eps3);         /**/

double EvalQuad(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2);         /**/

void EvalCAmpPhaseSpline(
  CAmpPhaseSpline* splines,                     //input
  CAmpPhaseFrequencySeries* freqseries);  //in/out defines CAmpPhase from defined freqs

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _SPLINECOEFFS_H */