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

/* INCLUDES */
#include "display.h"
#include "velodyneViewer.h"
#include "logger.h"
#include "drawingList.h"
#include "stereoCamera.h"
#include "colorEncoding.h"
#include "velodyneStream.h"
#include "velodyne.h"
#include "uShortImage.h"
#include "3DMatrix.h"

//#include "ippDefs.h"

using namespace VIC;

/// Constructors.
CVelodyneViewerOp::CVelodyneViewerOp ( COperator * const f_parent_p )
        : COperator (                      f_parent_p, "VelodyneViewer" ),
          m_birdViewScale  (                                   5.0, 5.0 ),
          m_birdViewOffset  (                                  0.0, 0.0 ),
          m_birdViewDispSize (                                     1, 1 ),
          m_testColor (                              255, 255, 255, 255 ),
          m_encodeMode_e (                                    CEM_RANGE ),
          m_colorEnc (   CColorEncoding::CET_HUE, S2D<float>(-0.5, 3.0) )
{
    /// Register drawing list.
    registerDrawingList ("Velodyne Left Image Overlay",
                         S2D<int> (0, 0),
                         true );

    registerDrawingList ("Velodyne Bird View",
                         S2D<int> (1, 1),
                         true );

    registerDrawingList ("Image Position Overlay",
                         S2D<int> (0, 0),
                         true );

    
    ADD_RGBA_PARAMETER( "Testing color parameter", 
                        "Some color tooltip.", 
                        m_testColor,
                        this,
                        TestColor, 
                        CVelodyneViewerOp );
    
    ADD_LINE_SEPARATOR;
    
    //BEGIN_PARAMETER_GROUP("Display Options", false, CColor::red );
    
    m_colorEnc.addParameters ( m_paramSet_p,
                               "Bird-view Color Encoding", 
                               "Color encoding for the Bird-view representation." );

    CEnumParameter<EColorEncodeMode_t> * param_p = static_cast<CEnumParameter<EColorEncodeMode_t> * > (
        ADD_ENUM_PARAMETER( "Color encoding mode",
                            "Set the color encoding mode for bird-view representation.",
                            EColorEncodeMode_t,
                            m_encodeMode_e,
                            this,
                            ColorEncodeMode,
                            CVelodyneViewerOp ) );

    param_p -> addDescription ( CEM_X,         "Local X" );
    param_p -> addDescription ( CEM_Y,         "Local Y" );
    param_p -> addDescription ( CEM_Z,         "Local Z" );
    param_p -> addDescription ( CEM_AZIMUTH,   "Azimuth" );
    param_p -> addDescription ( CEM_ELEVATION, "Elevation" );
    param_p -> addDescription ( CEM_RANGE,     "Range" );    
    
    CDrawingList *list_p;
    list_p = getDrawingList("Velodyne Left Image Overlay");
    ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                             "Overlay of velodyne data over the left stereo image.",
                             list_p -> getDisplayState(),
                             list_p,
                             DisplayState,
                             CDrawingList );

    list_p = getDrawingList("Velodyne Bird View");
    ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                             "Bird-view display of velodyne data.",
                             list_p -> getDisplayState(),
                             list_p,
                             DisplayState,
                             CDrawingList );

    list_p = getDrawingList("Image Position Overlay");
    ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                             "Position on the left image of the position mark by the "
                             "mouse on the bird-view",
                             list_p -> getDisplayState(),
                             list_p,
                             DisplayState,
                             CDrawingList );
      
    END_PARAMETER_GROUP;
}

/// Virtual destructor.
CVelodyneViewerOp::~CVelodyneViewerOp ()
{
}

/// Init event.
bool CVelodyneViewerOp::initialize()
{    
    return COperator::initialize();
}

/// Reset event.
bool CVelodyneViewerOp::reset()
{
    return COperator::reset();
}

bool CVelodyneViewerOp::exit()
{
    return COperator::exit();
}

/// Cycle event.
bool CVelodyneViewerOp::cycle()
{
    static S2D<int> birdViewPos = getDrawingList ("Velodyne Bird View") -> getPosition();
    
    registerOutput ( "Velodyne Viewer - Scale",              &m_birdViewScale );
    registerOutput ( "Velodyne Viewer - Display Size",       &m_birdViewDispSize );
    //registerOutput ( "Velodyne Viewer - Bird View Position", &birdViewPos );

    return COperator::cycle();
}

/// Show event.
bool CVelodyneViewerOp::show()
{
    const CVelodyneDataStream * stream_p = 
        dynamic_cast<CVelodyneDataStream*> ( getInput( "Velodyne Data Stream" ) );

    const CVelodyne * velodyne_p = 
        dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );
    
    /// Obtain input from parent.
    CCamera * cam_p       = dynamic_cast<CCamera *>(getInput ( "Camera" ) );
    CUShortImage  * img_p = dynamic_cast<CUShortImage  *>(getInput ( "Rectified Left Image" ) );

    if (!cam_p || !img_p || !stream_p || !velodyne_p )
        return false;
    
    int dispWidth_i  = 800;
    int dispHeight_i = 600;
    
    CVelodyneDataStream::const_reverse_iterator it  = stream_p->rbegin();
    CVelodyneDataStream::const_reverse_iterator end = stream_p->rend();
    
    C3DVector localPoint, worldPoint;
    double u_f, v_f;
        
    CDrawingList *list1_p,  *list2_p;

    SRgb  colorLine;

    list1_p = getDrawingList ("Velodyne Left Image Overlay");
    list1_p -> clear();
    list2_p = getDrawingList ("Velodyne Bird View");
    list2_p -> clear();

    //printf("Amount of Velodyne points is %i\n", stream_p->size());
    
    for (; it < end; ++it)
    {
        if (it->range_d != 0)
        {
            velodyne_p -> spherical2Local ( it->range_d,
                                            it->azimuth_d,
                                            it->elevation_d,
                                            localPoint );
            velodyne_p -> local2World ( localPoint,
                                        worldPoint );

            if (int(m_encodeMode_e) < 3)
                m_colorEnc.colorFromValue ( localPoint.at((int)m_encodeMode_e), colorLine );
            else
                m_colorEnc.colorFromValue ( it->elem_p[(int)m_encodeMode_e - (int)(CEM_RANGE)], colorLine );

            localPoint.at(0) += m_birdViewOffset.x;
            localPoint.at(2) += m_birdViewOffset.y;

            u_f = localPoint.x() * m_birdViewScale.x + (m_birdViewDispSize.x*dispWidth_i)/2;
            v_f = (m_birdViewDispSize.y*dispHeight_i)/2 - localPoint.z() * m_birdViewScale.y;

            if ( u_f >= 0 && u_f < m_birdViewDispSize.x*dispWidth_i &&
                 v_f >= 0 && v_f < m_birdViewDispSize.y*dispHeight_i )
            {
                list2_p -> setLineColor ( SRgba( colorLine, 255 ) );
                list2_p -> setFillColor ( SRgba( colorLine, 20) );
                list2_p -> addFilledRectangle ( u_f-.5, v_f-.5, u_f + .5, v_f + .5);
            }

            cam_p -> world2Image ( worldPoint.x(), 
                                   worldPoint.y(), 
                                   worldPoint.z(), 
                                   u_f, 
                                   v_f );
            
            //printf("GREP1 %f %f %f -> (%f %f)\n", x, y, z, u_f, v_f);

            if (worldPoint.z() > 0)
            {
                if ( u_f >= 0 && u_f < dispWidth_i &&
                     v_f >= 0 && v_f < dispHeight_i )
                {
                    list1_p -> setLineColor ( SRgba( colorLine, 255 ) );
                    list1_p -> setFillColor ( SRgba( colorLine, 70) );
                    list1_p -> addFilledRectangle ( u_f-1.5, v_f-1.5, u_f + 1.5, v_f + 1.5);
                }
            }
            
        }
    }

    float maxDist_f = std::min ( (dispWidth_i *m_birdViewDispSize.x/2.)/m_birdViewScale.x,
                                 (dispHeight_i*m_birdViewDispSize.y/2.)/m_birdViewScale.y );

    for (float i = 5 ; i <= maxDist_f; i+=5.)
    {
        list2_p -> setLineColor ( SRgba( 255, 255, 255, 50 ) );
        list2_p -> addEllipse ( (dispWidth_i  * m_birdViewDispSize.x/2.) + m_birdViewOffset.x * m_birdViewScale.x,
                                dispHeight_i * m_birdViewDispSize.y/2. - m_birdViewOffset.y * m_birdViewScale.y,
                                i * m_birdViewDispSize.x * m_birdViewScale.x, 
                                i * m_birdViewDispSize.y * m_birdViewScale.y );
    }
    
    CUShortImage * imgL_p = dynamic_cast<CUShortImage *>(getInput ( "Rectified Left Image" ) );
    list1_p = getDrawingList ("Preview Top-Left corner Image");
    list1_p -> clear();
    list1_p -> addImage ( *imgL_p, 10, 10,
                          160, 120 );

    return COperator::show();
}

void 
CVelodyneViewerOp::keyPressed ( CKeyEvent * f_event_p )
{
    if ( f_event_p -> qtKeyEvent_p -> key() == Qt::Key_R ) 
    {
        m_birdViewScale   = S2D<float>(5.f,5.f);
        m_birdViewOffset  = S2D<float>(0.f,0.f);

        show();
        updateDisplay();
    }
    
    return COperator::keyPressed ( f_event_p );   

}

void 
CVelodyneViewerOp::mouseMoved ( CMouseEvent * f_event_p )
{
    static S2D<int> prevPos;
    if (f_event_p -> displayScreen == getDrawingList ("Velodyne Bird View")->getPosition())
    {
        if ((f_event_p -> qtMouseEvent_p -> buttons() & Qt::LeftButton) != 0)
        {
            if (f_event_p -> qtMouseEvent_p -> modifiers() & Qt::ShiftModifier )
            {
                float deltaX_f = (f_event_p -> qtMouseEvent_p -> x() - prevPos.x);
                float deltaY_f = (f_event_p -> qtMouseEvent_p -> y() - prevPos.y);

                if (f_event_p -> qtMouseEvent_p -> modifiers() & Qt::AltModifier )
                {
                    m_birdViewOffset.x += deltaX_f/m_birdViewScale.x;
                    m_birdViewOffset.y -= deltaY_f/m_birdViewScale.y;
                }
                else
                    if (prevPos.isValid())
                    {
                        float motion_f = (deltaY_f + deltaX_f)/10.;
                        m_birdViewScale.x = std::max( 1.f, motion_f + m_birdViewScale.x);
                        m_birdViewScale.y = m_birdViewScale.x;
                    }
                show();
                updateDisplay();
            }
            else
            {
                const CFloatImage *    disp_p = dynamic_cast<CFloatImage *>(getInput ( "Disparity Image" ) );

                if ( disp_p )
                {
                    
                    const CStereoCamera *  cam_p  = dynamic_cast<CStereoCamera *>(getInput ( "Camera" ) );
                    const CStereoCamera *  rcam_p = dynamic_cast<CStereoCamera *>(getInput ( "Rectified Camera" ) );
                    const CVelodyne * velodyne_p  = dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );

                    int dispWidth_i  = 800;
                    int dispHeight_i = 600;
        
                    CDrawingList *  list_p;
                    list_p = getDrawingList ("Velodyne Bird View");

                    if ( list_p -> isVisible ( ) )
                    {
                        S2D<int> drawPos = list_p -> getPosition ( );
        
                        if ( f_event_p->displayScreen.x >= drawPos.x  && 
                             f_event_p->displayScreen.x < drawPos.x + m_birdViewDispSize.x &&
                             f_event_p->displayScreen.y >= drawPos.y  && 
                             f_event_p->displayScreen.y < drawPos.y + m_birdViewDispSize.y )
                        {
                            double x_f = ( (f_event_p->posInDisplay.x - (drawPos.x * dispWidth_i)) -
                                           (dispWidth_i * m_birdViewDispSize.x)/2 ) / m_birdViewScale.x - m_birdViewOffset.x;
            
                            double z_f = -( (f_event_p->posInDisplay.y - (drawPos.y * dispHeight_i)) -
                                            (dispHeight_i*m_birdViewDispSize.y)/2 ) / m_birdViewScale.y  - m_birdViewOffset.y;
            
                            double y_f = -1.55;

                            //double u_f, v_f;
            
                            double u_p[8];
                            double v_p[8];
                            int idx_i = 0;
            
                            //cam_p -> world2Image ( x_f, y_f, z_f, u_f, v_f );

                            if ( z_f > 0 ) //&& u_f > 0 && v_f > 0 && u_f <= img_p->getWidth() && v_f <= img_p->getHeight())
                            {
                                double deltaHeight_f = -.75;
                                double deltaX_f = 0.2;
                                double deltaZ_f = 0.2;
                    
                                C3DVector point, worldPoint;
                                for (; deltaHeight_f <= .75; deltaHeight_f+=1.5)
                                {
                                    point.set ( x_f-deltaX_f,
                                                y_f+deltaHeight_f,
                                                z_f-deltaZ_f );
                        
                                    velodyne_p -> local2World ( point, worldPoint );
                                    cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(), u_p[idx_i], v_p[idx_i]);
                                    ++idx_i;
                      
                                    point.set ( x_f-deltaX_f,
                                                y_f+deltaHeight_f,
                                                z_f+deltaZ_f );

                                    velodyne_p -> local2World ( point, worldPoint );
                                    cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                                    ++idx_i;
                        
                                    point.set ( x_f+deltaX_f,
                                                y_f+deltaHeight_f,
                                                z_f+deltaZ_f );

                                    velodyne_p -> local2World ( point, worldPoint );
                                    cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                                    ++idx_i;

                                    point.set ( x_f+deltaX_f,
                                                y_f+deltaHeight_f,
                                                z_f-deltaZ_f );

                                    velodyne_p -> local2World ( point, worldPoint );
                                    cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                                    ++idx_i;                
                                }

                                list_p =  getDrawingList ("Image Position Overlay" );
                                list_p -> clear();

                                deltaHeight_f = 0.75;

                                float *d_p = (float *) disp_p ->getData();

                                list_p -> setLineColor ( SRgba(255, 0, 0, 255 ) );

                                float scaleFactor_f = getCastedInputObject<CIO_float, float> ("Rectification Scale Factor", 1.f );
                        

                                for (unsigned int i = 0; i < disp_p -> getHeight(); ++i)
                                {
                                    for (unsigned int j = 0; j < disp_p -> getWidth(); ++j, ++d_p)
                                    {
                                        if (*d_p > 0)
                                        {
                                            rcam_p -> image2World ( j, i, *d_p, 
                                                                    worldPoint.at(0), 
                                                                    worldPoint.at(1), 
                                                                    worldPoint.at(2) );

                                            velodyne_p -> world2Local ( worldPoint, point );

                                            //printf("%i %i %f %f (%f %f) %f (%f %f) %f (%f %f)\n", j, i, *d_p, tx, x_f-deltaX_f, x_f+deltaX_f, ty, y_f-deltaHeight_f, y_f+deltaHeight_f, 
                                            //       tz, z_f-deltaZ_f, z_f+deltaZ_f );

                                            if ( point.x() > x_f-deltaX_f && 
                                                 point.x() < x_f+deltaX_f && 
                                                 point.y() > y_f-deltaHeight_f && 
                                                 point.y() < y_f+deltaHeight_f && 
                                                 point.z() > z_f-deltaZ_f && 
                                                 point.z() < z_f+deltaZ_f )
                                            {
                                                //printf("j = %i i = %i y: %f ref: %f\n", j, i, ty, y_f);
                                                list_p -> addRectangle ( (j-.5)/scaleFactor_f, (i-.5)/scaleFactor_f,
                                                                         (j+.5)/scaleFactor_f, (i+.5)/scaleFactor_f );
                                            }
                                        }
                                    }
                                }
                
                                /*            
                                              printf("World x: %f y: %f z: %f -> u: %f v: %f d: %f (%f %f)\n",
                                              x_f, y_f, z_f, u_f, v_f, d_f, f_event_p->posInScreen.x, f_event_p->posInScreen.y );
                                */
                
                                for (int i = 0; i < 4; ++i)
                                {
                                    list_p -> setLineColor ( CColor::red  );
                                    list_p -> addLine ( u_p[i], v_p[i], u_p[(i+1)%4], v_p[(i+1)%4] );
                                    list_p -> setLineColor (  CColor::blue );
                                    list_p -> addLine ( u_p[i+4], v_p[i+4], u_p[(i+1)%4+4], v_p[(i+1)%4+4] );
                                    list_p -> setLineColor ( CColor::green );
                                    list_p -> addLine ( u_p[i], v_p[i], u_p[(i+4)], v_p[(i+4)] );
                                }
                
                                /*
                                  printf("Screen is %i %i Pos is %f %f Local pos is %f %f\n", 
                                  f_event_p->displayScreen.x,
                                  f_event_p->displayScreen.y,
                                  f_event_p->posInDisplay.x,
                                  f_event_p->posInDisplay.y,
                                  f_event_p->posInScreen.x,
                                  f_event_p->posInScreen.y );
                                */
                            }
                
                            updateDisplay();
                        }
                        else
                        {
                        }
                    }
                }
            }
        }
    }
    
    prevPos.x = f_event_p -> qtMouseEvent_p -> x();
    prevPos.y = f_event_p -> qtMouseEvent_p -> y();

    return COperator::mouseMoved ( f_event_p );
}

