#include "IO_pose_vector.h"

#include <fstream>
#include <sstream>

//-----------------------------------------------------------------------------

namespace graphene
{
namespace character
{

//-----------------------------------------------------------------------------

bool read_pose_vector(const std::string& filename, std::vector<Mat4f>& pose_vector)
{
    pose_vector.clear();

    std::ifstream ifs(filename);
    if (!ifs)
    {
        printf("[ERROR] Cannot read %s\n", filename.c_str());
        return false;
    }

    float col0, col1, col2, col3;

    Mat4f m;
    int row = 0;

    std::string line;
    while (std::getline(ifs, line))
    {
        std::stringstream linestream(line);

        linestream >> col0 >> col1 >> col2 >> col3;

        if (linestream.fail())
        {
            printf("[ERROR] While parsing line %s\n", line.c_str());
            return false;
        }

        m(row, 0) = col0;
        m(row, 1) = col1;
        m(row, 2) = col2;
        m(row, 3) = col3;

        row++;

        if (row == 4)
        {
            pose_vector.push_back(m);
            row = 0;
        }
    }

    return true;
}

//-----------------------------------------------------------------------------

void print_pose_vector_angles(const std::vector<Mat4f>& pose_vector)
{
    size_t num_joints = pose_vector.size();

    for (size_t i = 0; i < num_joints; ++i)
    {
        Mat4f m = pose_vector[i];

        Vec3f euler = Mat4f::to_euler_XYZ(m);
        euler *= 180.0f / M_PI;

        printf("%f\n%f\n%f\n",
               euler[0], euler[1], euler[2]);
    }
}

//-----------------------------------------------------------------------------
}
}
//-----------------------------------------------------------------------------
