/*@@@**************************************************************************
* \file  velodyneReaderop
* \author Hernan Badino
* \date  Wed Apr  8 14:27:15 GMT 2009
* \notes 
*******************************************************************************
******************************************************************************/

/* INCLUDES */
#include "rigidMotionWidget.h"

#include "velodyneReader.h"
#include "logger.h"
#include "colorEncoding.h"

//#include "ippDefs.h"

using namespace VIC;

/// Constructors.
CVelodyneReaderOp::CVelodyneReaderOp ( COperator * const f_parent_p )
        : COperator ( f_parent_p, "VelodyneReader" ),
          m_bufferSize_ui (                  16384 ),
          m_buffer_p (                        NULL ),
          m_file_p  (                         NULL ),
          m_dialog_p (                        NULL ),
          m_deltaTime_d (                      0.1 ),
          m_show3DPoints_b (                 false ),
          m_roiTL (                   -M_PI, -0.02 ),
          m_roiSize (                  2*M_PI, 0.5 )

{
    m_buffer_p = new SVelodynePointData[m_bufferSize_ui];
    assert ( m_buffer_p );

    ADD_DOUBLE_PARAMETER ( "Delta Time", 
                           "Accumulation Time for Velodyne Data. It will be accumulated "
                           "all the data from time CurrTimeStamp-DaltaTime to time "
                           "CurrTimeStamp",
                           m_deltaTime_d,
                           this,
                           DeltaTime, 
                           CVelodyneReaderOp );

    ADD_BOOL_PARAMETER ( "Show 3D Points", 
                         "Accumulation Time for Velodyne Data. It will be accumulated "
                         "all the data from time CurrTimeStamp-DaltaTime to time "
                         "CurrTimeStamp",
                         m_show3DPoints_b,
                         this,
                         Show3DPoints, 
                         CVelodyneReaderOp );

    ADD_DBL2D_PARAMETER( "ROI Top-Left", 
                         "ROI top left for 3D point display.",
                         m_roiTL,
                         "Azimuth", "Elevation",
                         this,
                         RoiTL, 
                         CVelodyneReaderOp );

    ADD_DBL2D_PARAMETER( "ROI Size", 
                         "ROI Size for 3D point display.",
                         m_roiSize,
                         "Azimuth", "Elevation",
                         this,
                         RoiSize, 
                         CVelodyneReaderOp );
}

/// Virtual destructor.
CVelodyneReaderOp::~CVelodyneReaderOp ()
{
    if (m_buffer_p)
        delete [] m_buffer_p;

    if (m_dialog_p)
        delete m_dialog_p;
}

/// Init event.
bool CVelodyneReaderOp::initialize()
{
    /// Start rigid motion input widget.
    if ( !m_dialog_p )
        m_dialog_p = new CRigidMotionWidget ();

    /// Load only if not already initialized.
    if ( m_index_v.size() == 0 )
    {
        printf("Loading Velodyne Data\n");
        
        std::string f_filePath_str = 
            getCastedInputObject<CIOString, std::string> ("Image 0 Path", "");

        int pos_i = f_filePath_str.find_last_of ("/\\");

        if ( pos_i != -1 )
            f_filePath_str.erase( pos_i );
        else
            f_filePath_str = std::string("."); 

        f_filePath_str += "/velodyneData.dat";

        m_file_p = fopen ( f_filePath_str.c_str(), "r");
        printf("init m_file_p is %p (%s)\n", m_file_p, f_filePath_str.c_str());

        if (!m_file_p)
        {
            logger::warn("LUTs file \"velodyneData.dat\" was not found");
            return false;
        }
        else
        {
            size_t fieldsRead_i;
            
            size_t strDataSize_i = 1024;
            char *strData_p = (char *)malloc(1024);

            //fseek(m_file_p, 0, SEEK_END);
            //m_fileSize_ui = ftell(m_file_p) + 1;
            //rewind(m_file_p);

            fieldsRead_i = fread( strData_p, 1, 1, m_file_p );

            printf("Comments are:\n");

            /// Check for comments.
            while (*strData_p == '#')
            {
                fieldsRead_i = getline ( &strData_p, &strDataSize_i, m_file_p );
                printf( "%s\n", strData_p );
                fieldsRead_i = fread   ( m_buffer_p, 1, 1, m_file_p );
            }

            free(strData_p);

            fseek(m_file_p, -1, SEEK_CUR);        

            SVelodynePointData data;

            while (!feof(m_file_p))
            {
                fieldsRead_i = fread ( &data, sizeof(SVelodynePointData), 1, m_file_p);

                //printf("Range %f Az %f El %f\n", data.range_d, data.azimuth_d, data.elevation_d);                

                if (fieldsRead_i != 1)
                {
                    logger::warn("Data could not be read from file");
                }


                if (m_index_v.size() == 0 || 
                    m_index_v.back().timeStamp_d != data.timeStamp_d )
                {
                    SIndex idx;
                    idx.timeStamp_d = data.timeStamp_d;
                    idx.filePos_ui  = ftell(m_file_p) - sizeof(SVelodynePointData);
                    idx.lines_ui    = 1;
                    m_index_v.push_back(idx);
                }
                else
                    ++m_index_v.back().lines_ui;
            }
        }
    }
    printf("TOTAL NUMBER OF INDICES IS %i\n", m_index_v.size() );
    
    return COperator::initialize();
}

/// Reset event.
bool CVelodyneReaderOp::reset()
{
    logger::msg("Reset called");
    return COperator::reset();
}

bool CVelodyneReaderOp::exit()
{
    if ( m_file_p )
        fclose( m_file_p );

    return COperator::exit();
}

/// Cycle event.
bool CVelodyneReaderOp::cycle()
{
    applyTransformation();

    double tt_d = getCastedInputObject<CIO_double, double> ("Image 0 Timestamp", 0.);
    
    printf("m_file_p is %p\n", m_file_p);
    if ( !m_file_p || tt_d == -1 ) return false;
    
    printf("Time Stamp of LEFT Image is %lf\n", tt_d);

    std::vector<SIndex>::const_iterator it    = m_index_v.begin();
    std::vector<SIndex>::const_iterator endit = m_index_v.end();
    std::vector<SIndex>::const_iterator start;
    
    /// Find first element within the time interval.
    for (; it != endit; ++it)
    {
        if ( (*it).timeStamp_d <= tt_d && 
             (*it).timeStamp_d >= tt_d - m_deltaTime_d )
            break;
    }

    if (it == endit)
    {
        logger::warn ( "No velodyne data was between times %lf sec and %lf sec\n");
        m_stream.clear();
        registerOutput ( "Velodyne Data Stream",  &m_stream );
        return COperator::cycle();
    }

    /// Find last element within the time interval and accumulate lines to read.
    start = it;
    unsigned int registers_ui = (*it).lines_ui;
    for (; it != endit; ++it)
    {
        if ( !( (*it).timeStamp_d <= tt_d && 
                (*it).timeStamp_d >= tt_d - m_deltaTime_d ) )
            break;
        registers_ui += (*it).lines_ui;
    }
    
    /// Read now.
    printf("Number of registers to read is %i\n", registers_ui );
    
    /// Realloc if required.
    if ( m_bufferSize_ui < registers_ui )
    {
        m_bufferSize_ui = registers_ui;
        delete [] m_buffer_p;
        m_buffer_p = new SVelodynePointData[m_bufferSize_ui];
        assert ( m_buffer_p );
    }
    
    fseek(m_file_p, start->filePos_ui, SEEK_SET);
    unsigned int regRead_ui = fread( m_buffer_p, sizeof(SVelodynePointData), registers_ui, m_file_p );

    if (regRead_ui != registers_ui)
    {
        logger::warn("Velodyne data could not correctly be read");
        return false;
    }

    m_stream.clear();
    
    for (unsigned int i = 0; i < registers_ui; ++i)
    {
        m_stream.push_back( m_buffer_p[i] );
    }    

    /// Register outputs.
    registerOutput ( "Velodyne Data Stream",  &m_stream );
    registerOutput ( "Velodyne Sensor",       &m_velodyne );

    return COperator::cycle();

}

/// Apply rotation and translation.
void CVelodyneReaderOp::applyTransformation()
{
    
    double rot_p[3][3] = { {0.9935683662767510, 0.0172747092004820, -0.1119083819746910 },
                           {0.0041635084487756, 0.9820529070954280, 0.1885596798438500},
                           {0.1131572654828300, -0.1878128645419600, 0.9756647791024320 } } ;
    double trans_p[3] = {  0.6775398573062020, 0.3484528762820530, -0.2608627806075020 };
    
    C3DMatrix rotation    ( *rot_p);
    C3DVector translation ( trans_p );
    
    rotation    *= m_dialog_p -> getRotationMatrix();
    translation += m_dialog_p -> getTranslationVector();

    m_velodyne.setRotation    ( rotation );
    m_velodyne.setTranslation ( translation );

    if (0)
    {
        static double move_d;
        move_d += M_PI/315;
        //printf("move_d = %f (%f)\n", move_d, move_d/3.1415926);
        
        rotation.loadIdentity();
        //rotation.rotateX(move_d);
        //rotation.rotateY(0.296705968);//random()/(double)RAND_MAX * 2*M_PI);
        translation.clear();
        
        m_velodyne.setRotation    ( rotation );
        m_velodyne.setTranslation ( translation );
    }
}

/// Show event.
bool CVelodyneReaderOp::show()
{
#if defined HAVE_QGLVIEWER
    if ( m_3dViewer_p && m_show3DPoints_b )
    {
        CColorEncoding colorEnc ( CColorEncoding::CET_HUE, S2D<float>(4.,30.) );

        const C3DVector vec(0,0,0);
        SRgb color ( 0, 255, 0);
        SSphericalPointData meas;

        for (int i = 0; i < (int)m_stream.size(); ++i)
        {
            C3DVector p;
            
            if ( m_stream[i].azimuth_d > m_roiTL.x && 
                 m_stream[i].azimuth_d <= m_roiTL.x+m_roiSize.width && 
                 m_stream[i].elevation_d > m_roiTL.y && 
                 m_stream[i].elevation_d <= m_roiTL.y+m_roiSize.height )
            {
                colorEnc.colorFromValue ( m_stream[i].range_d,
                                          color );

                if (0)
                    m_velodyne.spherical2Local ( m_stream[i].range_d,
                                                 m_stream[i].azimuth_d,
                                                 m_stream[i].elevation_d,
                                                 p );
                else
                    m_velodyne.spherical2World ( m_stream[i].range_d,
                                                 m_stream[i].azimuth_d,
                                                 m_stream[i].elevation_d,
                                                 p );
                
                m_3dViewer_p -> addPoint ( p, color, vec );
            }
        }
    }

#endif
    


    return COperator::show();
}

void 
CVelodyneReaderOp::keyPressed ( CKeyEvent * f_event_p )
{
   return COperator::keyPressed ( f_event_p );    
}

std::vector<QWidget*> 
CVelodyneReaderOp::getWidgets( ) const
{
    /// Get first widgets from the children.
    std::vector<QWidget*> result = COperator::getWidgets();

    if (m_dialog_p)
        result.push_back(m_dialog_p);
    return result;
}

