//#####################################################################
// Copyright 2020.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################

#include <Core/Arrays/ARRAY.h>
#include <Core/Log/LOG.h>
#include <Core/Matrices/MATRIX.h>
#include <Core/Matrices/MATRIX_MXN.h>
#include <Core/Matrices/ROTATION.h>
#include <Core/Matrices/SYMMETRIC_MATRIX.h>
#include <Core/Vectors/VECTOR.h>
#include "OP_COUNT.h"

using namespace PhysBAM;

// TASK 1: read through the core matrix classes, which are in MATRIX.h,
// MATRIX_MXN.h, MATRIX_BASE.h, MATRIX_3X3.h, MATRIX_3X2.h,
// MATRIX_2X2.h, MATRIX_1X1.h, MATRIX_0X0.h.  As before, read through them
// quickly to get a general sense for what is in them and how to interface with
// them.  Note that MATRIX_MXN is dynamic and MATRIX is static.  Both derive
// from MATRIX_BASE, which provides common functionality.  The other files are
// specializations to make small sizes faster, in much the same way we saw for
// vectors.  These are the general dense matrix classes in PhysBAM.

// PhysBAM also has specialized matrix classes.  Of these, the ones you are most
// likely to encounter are DIAGONAL_MATRIX<T,m> and SYMMETRIC_MATRIX<T,m>.
// The ones with a
// size as a template argument are fixed-size.  The others are dynamic.  Most of
// the fixed types have size-specific specializations.

int TASK=-1;

template<class A,class B>
void Test(const A& a,const B& b)
{
    if(TASK>=0) std::cout<<"TASK: "<<TASK<<std::endl;
    auto aa=LOG::sprintf("%P",a);
    auto bb=LOG::sprintf("%P",b);
    std::cout<<"should be: "<<bb<<std::endl;
    std::cout<<"computed : "<<aa<<std::endl;
    if(aa==bb) std::cout<<"SAME\n"<<std::endl;
    else std::cout<<"DIFFERENT\n"<<std::endl;
    TASK=-1;
}

int main(int argc, char* argv[])
{
    typedef double T;
    typedef VECTOR<T,3> TV;

    // Replace the TODO lines with suitable code to pass the tests that follow.
    // Each task can be accomplished with one statement and no loops.

    TASK=2;
    MATRIX<T,3,2> m0; // TODO
    Test(m0,"[0 2; 1 3; 1 5]");

    TASK=3;
    MATRIX_MXN<T> m1; // TODO
    Test(m1,"[0 0 0; 0 0 0]");

    TASK=4;
    // TODO
    Test(m1,"[0 1 1; 2 3 5]");

    TASK=5;
    // TODO
    Test(m1,"[0 2 0 0; 1 3 0 0; 1 5 0 0; 0 0 0 0]");

    TASK=6;
    // TODO
    Test(m1,"[0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0]");

    TASK=7;
    // TODO
    Test(m1,"[4 0 0 0; 0 4 0 0; 0 0 4 0; 0 0 0 4]");

    TASK=8;
    // TODO
    Test(m1,"[4 0 0 0; 0 0 2 0; 0 1 3 0; 0 1 5 4]");

    // These tasks may require more than one line.  Loops, branches, etc. are
    // all fine.
    TASK=9;
    auto l0=[](const ARRAY<T>& u)
        {
            return 0; // TODO
        };
    Test(l0({1,1,1}),"[1 1 1; 1 1 1; 1 1 1]");
    Test(l0({1,2,3}),"[1 3 2; 2 1 3; 3 2 1]");
    Test(l0({7}),"[7]");
    Test(l0({2,4,9,3,7}),"[2 7 3 9 4; 4 2 7 3 9; 9 4 2 7 3; 3 9 4 2 7; 7 3 9 4 2]");

    TASK=10;
    auto l1=[](const TV& u, const TV& v)
        {
            // Compute and return I-u*v^T-v*u^T as a symmetric matrix.  This is
            // a one-liner.
            return SYMMETRIC_MATRIX<T,3>(); // TODO
        };
    SYMMETRIC_MATRIX<T,3> sm0;
    sm0=l1({0,0,1},{1,0,0});
    Test(sm0,"[1 0 -1 ; 0 1 0 ; -1 0 1]");
    sm0=l1({1,2,3},{-1,2,0});
    Test(sm0,"[3 -0 3 ; -0 -7 -6 ; 3 -6 1]");

    TASK=11;
    auto l2=[](const MATRIX<T,3>& m,T s)
        {
            // Compute and return m+m^T-s*m^T*m as a symmetric matrix.  This is
            // a one-liner.
            return SYMMETRIC_MATRIX<T,3>(); // TODO
        };
    MATRIX<T,3> m2(0,1,2,3,4,5,6,7,8);
    sm0=l2(m2,-2);
    Test(sm0,"[10 32 54 ; 32 108 184 ; 54 184 314]");

    TASK=12;
    auto l3=[](const MATRIX<T,3>& m)
        {
            // Compute and return m*m^T+3*I as a symmetric matrix.  This is
            // a one-liner.
            return SYMMETRIC_MATRIX<T,3>(); // TODO
        };
    sm0=l3(m2);
    Test(sm0,"[48 54 63 ; 54 69 78 ; 63 78 96]");

    // PhysBAM has special routines for M^T*A and M*A^T, namely
    // M.Transpose_Times(A) and M.Times_Transpose(A).  These also work with
    // vectors, M^T*u = M.Transpose_Times(u).  This is potentially more
    // efficient than M.Transposed()*u because it avoids making a copy.
    Test(m2.Transpose_Times(TV{1,0,0}),"(0 3 6)");

    // Matrices define Rows() and Columns() to obtain the size.  The members m
    // and n also work, and for fixed sizes they are compile-time constants.
    // For symmetric matrices, it is only necessary to initialize one half of
    // the matrix, since only one is stored.  Using Element_Lower or
    // Element_Upper is more efficient than operator() because it avoids to need
    // to test which side of the diagonal is being filled.
    for(int i=0;i<sm0.Rows();i++)
        for(int j=0;j<=i;j++)
            sm0.Element_Lower(i,j)=i*10+j;
    Test(sm0,"[0 10 20 ; 10 11 21 ; 20 21 22]");

    // The following tasks are all one-liners.
    TASK=13;
    // Compute sm0 = m^T * sm0 * m.
    // TODO
    Test(sm0,"[183 687 1191 ; 687 2406 4125 ; 1191 4125 7059]");

    TASK=14;
    TV v0;
    // TODO
    Test(m2,"[0 3 6; 1 4 7; 2 5 8]");
    Test(v0,"(3 4 5)");

    TASK=15;
    // TODO
    Test(m2,"[0 3 6; 1 4 7; 2 5 8]");
    Test(v0,"(2 5 8)");

    TASK=15;
    // TODO
    Test(m2,"[0 3 6; 1 4 7; 6 4 6]");

    TASK=16;
    // TODO
    Test(m2,"[0 7.5 15; 2.5 10 17.5; 15 10 15]");

    TASK=17;
    v0={1,3,5};
    // TODO
    Test(sm0,"[1 3 5 ; 3 9 15 ; 5 15 25]");

    TASK=18;
    MATRIX<T,3> R;
    v0={1,2,3};
    auto R2 = ROTATION<TV>::From_Rotation_Vector(v0).Rotation_Matrix();
    // Implement this:
    // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
    // v0 is k*theta in the wiki page.  You can use more than one statement if
    // you like.  No loops.  No direct access of matrix or vector components.
    // TODO
    // This can actually be done using existing library constructs.  I will use
    // this as a double-check to make sure we have implemented Rodrigues'
    // rotation formula correctly.
    Test(R,LOG::sprintf("%P",R2));

    TV v2;
    {
        TV u(1,2,3),v(-2,0,-1);
        MATRIX<T,2,3> m((T)1,(T)0,(T)1,(T)2,(T)2,-(T)1);
        auto m3=Outer_Product(u,v)+Outer_Product(v,u)+m.Transposed()*m+MATRIX<T,3>::Identity_Matrix();
        v2=m3*u+m3*v+m.Transposed()*(m*m.Transposed()).Inverse()*m*u;
    }

    // Optimize this computation.  You are not allow to use loops or access
    // components of matrices or vectors directly.
    TASK=19;
    typedef VECTOR<OP_COUNT<double>,3> V3;
    typedef MATRIX<OP_COUNT<double>,2,3> M23;
    typedef MATRIX<OP_COUNT<double>,3> M3;
    V3 u(1,2,3),v(-2,0,-1);
    M23 m(1,0,1,2,2,-1);
    auto m3=Outer_Product(u,v)+Outer_Product(v,u)+m.Transposed()*m+M3::Identity_Matrix();
    auto v1=m3*u+m3*v+m.Transposed()*(m*m.Transposed()).Inverse()*m*u;
    Print_Op_Counts();
    LOG::printf("goal: add 55   mul 53   div 1   cmp 0   slow 0\n");
    Test(v1,v2);


    return 0;
}
