#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "transpose.h"

#define PI 3.1415

// utils
#define typecheck(type, var)  \
    (_Static_assert(__builtin_types_compatible_p(type, typeof(var)), "Incompatible Types!"))
#define MAX(a,b)               \
    ({  typeof(a) _a = (a);    \
        typeof(b) _b = (b);    \
        _a > _b ? _a : _b; })
#define MIN(a,b)               \
    ({  typeof(a) _a = (a);    \
        typeof(b) _b = (b);    \
        _a > _b ? _b : _a; })
#define ARR_LENGTH(arr)        \
    (sizeof((arr))/sizeof((arr)[0]))
#define CLOSEST_POW2(n)        \
    (1UL << (32 - __builtin_clz((unsigned int)((n) - 1))))
#define OVERLOAD_MACRO(_0, _1, _2, _3, _4, NAME, ...) NAME

// types for typedefing
#define VECTOR(vec, type, nmemb) \
    type vec##nmemb __attribute__((vector_size(CLOSEST_POW2(nmemb * sizeof(type)))))
#define MATRIX(mat, vec, n, m)   \
    struct { vec##m c[n]; } mat##n##x##m

// vector operations
#define VEC(...) {__VA_ARGS__}
#define vec_element(vec, n) ((vec)[(n)])
#define vec_promote(vec, type)                                  \
    ({  __builtin_choose_expr(                                  \
            __builtin_types_compatible_p(typeof(vec), type),    \
            *(type *)&vec,                                      \
            ({  type res = {0};                                 \
                memcpy(&res, &vec,                              \
                       MIN(sizeof(res), sizeof(vec)));          \
                res; }));                                       \
    })
#define vec_print2(vec, fmt)             \
    ({  printf("{ ");                   \
        for(size_t i = 0;               \
            i < ARR_LENGTH(vec); i++)   \
            printf(fmt" ", vec[i]);     \
        printf("}\n");                  \
    })
#define vec_print3(vec, fmt, nmemb)     \
    ({  printf("{ ");                   \
        for(size_t i = 0;               \
            i < nmemb; i++)             \
            printf(fmt" ", vec[i]);     \
        printf("}\n");                  \
    })
#define vec_print(...)                                \
    OVERLOAD_MACRO(_0, ##__VA_ARGS__, _4, vec_print3, \
                   vec_print2, _1, _0) (__VA_ARGS__)

// matrix operations
#define MAT(...) {{ TRANSPOSE(__VA_ARGS__) }}
#define mat_element(mat, n, m)  ((mat).c[(n)][(m)])
#define mat_column(mat, n) ((mat).c[(n)])
#define mat_promote(mat, type)                                          \
    ({  __builtin_choose_expr(                                          \
            __builtin_types_compatible_p(typeof(mat), type),            \
            *(type *)&mat,                                              \
            ({  type res = {0};                                         \
                for(size_t i = 0; i < MIN(ARR_LENGTH(mat.c),            \
                                          ARR_LENGTH(res.c)); i++)      \
                    res.c[i] = vec_promote(mat.c[i], typeof(res.c[i])); \
                res; }));                                               \
    })
#define mat_print4(mat, fmt, n, m)            \
    ({  for(int i = 0; i < m; i++) {          \
            printf("{ ");                     \
            for(int j = 0; j < n; j++)        \
                printf(fmt" ", mat.c[j][i]);  \
            printf("}\n");                    \
        }                                     \
    })
#define mat_print2(mat, fmt)                    \
    ({  size_t n = ARR_LENGHT((mat).c);         \
        size_t m = ARR_LENGHT((mat).c[0]);      \
        for(int i = 0; i < m; i++) {            \
            printf("{ ");                       \
            for(int j = 0; j < n; j++)          \
                printf(fmt" ", (mat).c[j][i]);  \
            printf("}\n");                      \
        }                                       \
    })
#define mat_print(...)                                \
    OVERLOAD_MACRO(_0, ##__VA_ARGS__, mat_print4, _3, \
                   mat_print2, _1, _0) (__VA_ARGS__)

// mixed operations
#define vecmat_mul(_v, _m)                                \
    ({  typeof(_m.c[0]) res = {0};                        \
        size_t n = MIN(ARR_LENGTH(_m.c), ARR_LENGTH((_v))); \
        for(size_t i = 0; i < n; i++)                     \
            res += _m.c[i] * _v[i];                       \
        res; })

typedef VECTOR(vec, double, 2);
typedef VECTOR(vec, double, 3);
typedef VECTOR(vec, double, 6);
typedef MATRIX(mat, vec, 2, 2);
typedef MATRIX(mat, vec, 3, 2);

typedef VECTOR(vec, double, 1);
typedef VECTOR(vec, double, 5);
typedef VECTOR(vec, double, 4);
typedef MATRIX(mat, vec, 3, 3);
typedef MATRIX(mat, vec, 2, 3);
typedef MATRIX(mat, vec, 3, 1);
typedef MATRIX(mat, vec, 4, 4);
typedef MATRIX(mat, vec, 5, 5);
typedef MATRIX(mat, vec, 4, 5);

// todo: smart current limit
//       trapezoid pid

struct pid_ctx {
    double Kp;
    double Ki;
    double Kd;
    
    double prev_e;
    double Ie;
};

double pid(struct pid_ctx *ctx, double measurement, double setpoint, double dt)
{
    double e = setpoint - measurement;
    double De = (e - ctx->prev_e) / dt;
    ctx->Ie += e * dt;
    ctx->prev_e = e;
    
    return
        ctx->Kp * e +
        ctx->Kd * De +
        ctx->Ki * ctx->Ie;
}

int brushed(void)
{
    char filename[] = "data1.dat";
    
    // PID
    double setpoint = 4;
    struct pid_ctx pid_velocity = {20.0, 0.0, 0.0, 0.0, 0.0};
    struct pid_ctx pid_position = {30.0, 0.0, 0.0};
    
    // times
    double dt = 0.00001;
    double tfinal = 0.5;
    
    // limits
    double voltage_max = 12;
    double current_max = 30;
    double torque_lose_rate = 1;

   
    // motor constants
    // double J  = 300 * 0.1; // rotor inertia [kg*m^2]
    // double b  =  10 * 0.5; // damping coefficient [-]
    // double Kt =  10 * 1.0; // torque constant [Nm/A]
    // double L  = 100 * 1.0; // inductance [H]
    // double R  =  20 * 5.0; // resistance [Ohm]
    // double Ke =  40 * 1.0; // back emf constant [V/m s^-1]
    
    double J  = 0.0044; // rotor inertia [kg*m^2]
    double b  = 0.0011; // damping coefficient [Nm*s/rad]
    double Kt = 0.22;   // torque constant [Nm/A]
    double L  = 0.01;   // inductance [H]
    double R  = 4;      // resistance [Ohm]
    double Ke = 0.22;   // back emf constant [Vs/rad]
    
    // state space
    mat3x3 A = MAT((0.0,  1.0,   0.0),
                   (0.0, -b/J,   Kt/J),
                   (0.0, -Ke/L, -R/L));
    
    mat2x3 B = MAT((0.0,   0.0),
                   (0.0,  -1.0/J),
                   (1.0/L, 0.0));
    
    // mat3x1 C = MAT((0.0, 1.0, 0.0));

    
    vec3 x = VEC(0.0, 0.0, 0.0);
    vec2 u = VEC(0.0, 0.0);

    // Simulation
    FILE *fp = fopen(filename, "w");
    if(!fp) return 1;
    
    for(double i = 0.0f; i < tfinal; i += dt) {
        // state space and euler integration
        vec3 xdot = vecmat_mul(x, A) + vecmat_mul(u, B);
        x += xdot * dt;        

        // pid control
        if((int)i*100000 % 1000 == 0) {
            double velocity_setpoint =
                pid(&pid_position, vec_element(x, 0), setpoint, dt);
            vec_element(u, 0) =
                pid(&pid_velocity, vec_element(x, 1), velocity_setpoint, dt);

        }
            
        // limits
        if(vec_element(u, 0) > voltage_max) vec_element(u, 0) = voltage_max;
        // if(vec_element(x, 2) > current_max) vec_element(x, 2) = current_max;

        // timed inputs
#define around(i, v) ((i) > ((v)-0.000005) && (i) < ((v)+0.000005))
#define torque_surge(t, torque) if(around(i, t)) vec_element(u, 1) = (torque)
        
        // torque limits
        if(vec_element(u, 1) < 0.0) vec_element(u, 1) = 0.0;
        if(vec_element(u, 1) > 0.0) vec_element(u, 1) -= torque_lose_rate;
                
        // torque_surge(0.025, 0.2);
        // torque_surge(0.075, 0.2);
        // torque_surge(0.100, 0.2);
        // torque_surge(0.250, 0.2);
        torque_surge(0.300, 20);

        // setpoint movement
        if(around(i, 2.25)) setpoint = 5;
        
        fprintf(fp, "%f %f %f %f %f %f\n", i,
                vec_element(x, 0),
                vec_element(x, 1),
                vec_element(x, 2),
                vec_element(u, 0),
                vec_element(u, 1));
    }

    fclose(fp);
    return 0;
}

double fa(double t) {return sin(t); }
double fb(double t) {return fa(t + (2.0/3.0 * PI)); }
double fc(double t) {return fb(t + (2.0/3.0 * PI)); }

int brushless()
{
    // motor constants
    double R = 1.0;
    double L = 1.1;
    double M = 1.0;
    double J = 1.0;
    double b = 1.0;
    double P = 2.0;
    double l = 1.0;

    // state space
#define L1      (L-M)
#define emfa(a) l/J*fa(a)
#define emfb(a) l/J*fb(a)
#define emfc(a) l/J*fc(a)
#define _SQRT3_2 (sqrt(3.0)/2.0)
    
#define A_MAT(a)                                    \
    MAT((-R/L1,   0,       0,       -emfa(a), 0),   \
        (0,      -R/L1,    0,       -emfb(a), 0),   \
        (0,       0,      -R/L1,    -emfc(a), 0),   \
        (emfa(a), emfb(a), emfc(a), -b/J,     0),   \
        (0,       0,       0,        P/2.0,   0))
    
#define emf_VEC(o, a) ((vec3)VEC(fa(a), fb(a), fc(a)) * l * o);

    mat5x5 A = A_MAT(0);

    mat4x5 B = MAT((1.0/L1, 0, 0, 0),
                   (0, 1.0/L1, 0, 0),
                   (0, 0, 1.0/L1, 0),
                   (0, 0, 0, 1.0/L1),
                   (0, 0, 0, 0));

    vec5 x   = VEC(0);
    vec4 u   = VEC(0);
    vec3 emf = emf_VEC(0, 0);

    mat2x3 clarke = MAT((1.0,   0.0),
                        (-0.5,  _SQRT3_2),
                        (-0.5, -_SQRT3_2));

    // simulation
    char filename[] = "data2.dat";
    double time = 100;
    double dt = 0.0001;

    double U = 0.001;
    
    FILE *fp = fopen(filename, "w");
    if(!fp) return 1;
    
    double i2 = 0;
    for(double i = 0; i < time; i += dt) {
        A   = (mat5x5) A_MAT(vec_element(x, 4));
        emf = (vec3)   emf_VEC(vec_element(x, 3), vec_element(x, 4));
        
        vec5 xdot = vecmat_mul(x, A) + vecmat_mul(u, B) + vec_promote(emf, vec5);
        x += xdot * dt;
        
        if((int)i*1000 % 100 == 0) {            
            vec3 voltages = vecmat_mul(
                U * (vec2)VEC(sin(vec_element(x, 4)),
                              cos(vec_element(x, 4))),
                clarke) + 0.001;

        
            // u = vec_promote(voltages, vec4);
            u = (vec4)VEC(0.0, 1.0, -1.0);
        }

        fprintf(fp, "%f %f %f %f %f %f %f %f %f %f\n", i,
                vec_element(x, 0),
                vec_element(x, 1),
                vec_element(x, 2),
                vec_element(x, 3),
                vec_element(x, 4),
                vec_element(u, 0),
                vec_element(u, 1),
                vec_element(u, 2),
                vec_element(u, 3));
    }
    
}

int main(void)
{
    // return brushed();
    return brushless();
}

void vec_example(void)
{
    // vec2 v2 = VEC(1.0, 2.5);
    // vec3 v3 = vec_promote(v2, typeof(v3));
    // vec_element(v3, 2) = 8;

    // mat3x2 m = MAT((1.0, 2.0, -1.0),
    //                (0.0, 1.0,  2.5));
    // mat_element(m, 0, 1) = 6.9;

    // vec2 r = vecmat_mul(v3, m);

    // vec_print(v3, "%.2f", 3);
    // printf("      times\n");
    // mat_print(m, "%.2f", 3, 2);
    // printf("        =\n");
    // vec_print(r, "%.2f", 2);
    
    // mat4x6 m2 = mat_promote(m,  typeof(m2));
    // printf("\npromoted bigger\n");
    // mat_print(m2, "%.2f", 4, 6);
    
    // mat2x2 m3 = mat_promote(m2, typeof(m3));
    // printf("\npromoted smaller\n");
    // mat_print(m3, "%.2f", 2, 2);

    // FILE *fp = fopen("test.dat", "w");
    // // fprintf(fp, "TIME, NUMBER1, NUMBER2\n");
    // for(size_t i = 0; i < 1000000; i++) {
    //     // fprintf(fp, "%ld,%ld,%ld\n", i, i % 256, i % 137);
    //     fprintf(fp, "%ld %ld %ld\n", i, i % 256, i % 137);
    //     fflush(fp);
    //     usleep(50000);
    // }
    // fclose(fp);


    vec2 _tv21 = {0};
    vec2 _tv22 = {1};
    vec2 _tv23 = _tv21 + _tv22;
    vec6 _tv61 = {0};
    vec6 _tv62 = {1};
    vec6 _tv63 = _tv61 + _tv62;
}