#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
using namespace std;

extern "C"
void dlaror_(const char* side, const char* init, int* m, int* n, double* a, int* lda,
             int* iseed, double* x, int* info);

extern "C"
void dorcsd2by1_(const char* jobu1, const char* jobu2, const char* jobv1t,
                 const int* m, const int* p, const int* q,
                 double* x11, const int* ldx11, double* x21,
                 const int* ldx21, double* theta, double* u1,
                 const int* ldu1, double* u2, const int* ldu2,
                 double* v1t, const int* ldv1t, double* work,
                 const int* lwork, int* iwork, int* info );

extern "C"
void dgemm_(const char *transa, const char *transb, const int *m,
            const int *n, const int *k,
            const double *alpha, const double *a, const int *lda,
            const double *b, const int *ldb,
            const double *beta, double *c, const int *ldc);

int main()
{
    int m = 260;
    int p = 130;
    int q = 131;
    int iseed[] = { 0, 1, 2, 3 };
    int info;
    
    double* a = new double[m*q];
    double* x = new double[2*m + q];
    dlaror_("L", "I", &m, &q, a, &m, iseed, x, &info);    

    int mp = m - p;
    double* x11 = new double[p*q];
    double* x12 = new double[mp*q];
    for (int i = 0; i < p; i++)
        for (int j = 0; j < q; j++)
            x11[j*p + i] = a[j*m + i];

    for (int i = p; i < m; i++)
        for (int j = 0; j < q; j++)
            x12[j*mp + i - p] = a[j*m + i];

    int r = min(p, min(m - p, min(q, m - q)));
    double* theta = new double[r];
    double* u1 = new double[p*p];
    double* u2 = new double[mp*mp];
    double* v1t = new double[q*q];
    int* iwork = new int[m - r];
    double dwork;
    int lwork = -1;
    dorcsd2by1_("Y", "Y", "Y", &m, &p, &q, x11, &p, x12, &mp, theta, u1, &p,
                u2, &mp, v1t, &q, &dwork, &lwork, iwork, &info);

    lwork = (int)dwork;
    double* work = new double[lwork];
    dorcsd2by1_("Y", "Y", "Y", &m, &p, &q, x11, &p, x12,  &mp, theta, u1, &p,
                u2, &mp, v1t, &q, work, &lwork, iwork, &info);
    
    double* unit = new double[mp*mp];
    double one = 1;
    double zero = 0;
    dgemm_("T", "N", &mp, &mp, &mp, &one, u2, &mp, u2, &mp, &zero, unit, &mp);
    
    double err = 0;
    for (int i = 0; i < mp; i++)
        for (int j = 0; j < mp; j++)
            err = max(err, abs((i == j ? 1 : 0) - unit[j*mp + i]));

    cout << "error = " << err << endl;
    
    delete[] a;
    delete[] x;
    delete[] work;
    delete[] iwork;
    delete[] x11;
    delete[] x12;
    delete[] theta;
    delete[] u1;
    delete[] u2;
    delete[] v1t;
    delete[] unit;
    return 0;
}