#include <cstdio>
#include <cstring>
#ifdef _WIN32
#include <windows.h>
#endif

#include "../Lib/opencpn/src/mygdal/ogr_s57.h"
#include "../Lib/opencpn/src/mygdal/cpl_conv.h"

void show_usage()
{
    fprintf( stderr, "Usage: encDump [-v] [-q] inFilename\n"
             "       -v verbose option turns on extra output\n"
             "       -d dump data along the way\n"
             "       -q quiet option turns off normal output\n" );
}

class SGND
{
public:
    SGND( int rcnm, int rcid )
        : rcnm_( rcnm ),
          rcid_( rcid ),
          length_( 0 ),
          x_( NULL ),
          y_( NULL ),
          used_( false ),
          next_( NULL ),
          reverse_( false )
    {}
    virtual ~SGND()
    {
        setLength( 0 );
        if( NULL != next_ )
        {
            delete next_;
        }
    }
    int getLength()
    {
        return length_;
    }
    int getRcnm()
    {
        return rcnm_;
    }
    int getRcid()
    {
        return rcid_;
    }
    virtual void setLength( int length )
    {
        if( length_ > 0 && NULL != x_ && NULL != y_ )
        {
            delete[] x_;
            x_ = NULL;

            delete[] y_;
            y_ = NULL;
        }
        length_ = length;
        if( length > 0 )
        {
            x_ = new int[length];
            y_ = new int[length];
            for( int i = 0; i < length; ++i )
            {
                x_[i] = y_[i] = 0x7FFFFFFF;
            }
        }
    }
    virtual bool check( int rcnm, int rcid )
    {
        return rcnm_ == rcnm && rcid_ == rcid;
    }
    virtual void set( int index, int x, int y )
    {
        if( index < length_ )
        {
            x_[index] = x;
            y_[index] = y;
        }
    }
    void setNext( SGND* next )
    {
        next_ = next;
    }
    int getX( int index )
    {
        return index < length_ ? x_[reverse_ ? length_ - 1 - index : index] : -1;
    }
    int getY( int index )
    {
        return index < length_ ? y_[reverse_ ? length_ - 1 - index : index] : -1;
    }
    void setUsed( bool used )
    {
        used_ = used;
    }
    bool isUsed()
    {
        return used_;
    }
    bool isReverse()
    {
        return reverse_;
    }
    void setReverse( const bool reverse )
    {
        reverse_ = reverse;
    }
protected:
    static SGND* Find( SGND* start, int rcnm, int rcid )
    {
        while( NULL != start )
        {
            if( start->check( rcnm, rcid ) )
            {
                return start;
            }
            start = start->next_;
        }
        return NULL;
    }
    int rcnm_;
    int rcid_;
    int length_;
    int* x_;
    int* y_;
    bool used_;
    SGND* next_;
    bool reverse_;
private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    SGND( const SGND& old ); // disallow copy constructor
};

class SG2D : public SGND
{
public:
    SG2D( int rcnm, int rcid )
        : SGND( rcnm, rcid ),
          startRcnm_( -1 ),
          startRcid_( -1 ),
          endRcnm_( -1 ),
          endRcid_( -1 ),
          depth_( nanf( "" ) )
    {}
    virtual void set( int index, int x, int y )
    {
        SGND::set( index, x, y );
        if( index == 0 )
        {
            startRcnm_ = startRcid_ = -1;
        }
        if( index == length_ - 1 )
        {
            endRcnm_ = endRcid_ = -1;
        }
    }
    bool done()
    {
        return length_ > 0 &&
               startRcnm_ == -1 && startRcid_ == -1 &&
               endRcnm_ == -1 && endRcid_ == -1;
    }
    bool getStart( int& rcnm, int& rcid )
    {
        if( startRcnm_ != -1 && startRcid_ != -1 )
        {
            rcnm = startRcnm_;
            rcid = startRcid_;
            return true;
        }
        return false;
    }
    bool getEnd( int& rcnm, int& rcid )
    {
        if( endRcnm_ != -1 && endRcid_ != -1 )
        {
            rcnm = endRcnm_;
            rcid = endRcid_;
            return true;
        }
        return false;
    }
    void setStart( int rcnm, int rcid )
    {
        startRcnm_ = rcnm;
        startRcid_ = rcid;
    }
    void setEnd( int rcnm, int rcid )
    {
        endRcnm_ = rcnm;
        endRcid_ = rcid;
    }
    bool checkStart( int rcnm, int rcid )
    {
        return startRcnm_ == rcnm && startRcid_ == rcid;
    }
    bool checkEnd( int rcnm, int rcid )
    {
        return endRcnm_ == rcnm && endRcid_ == rcid;
    }
    SG2D* getNext()
    {
        return ( SG2D* )next_;
    }
    static SG2D* Find( SG2D* start, int rcnm, int rcid )
    {
        return ( SG2D* )SGND::Find( start, rcnm, rcid );
    }
    float getDepth()
    {
        return depth_;
    }
    void setDepth( const float depth )
    {
        depth_ = depth;
    }
private:
    int startRcnm_;
    int startRcid_;
    int endRcnm_;
    int endRcid_;
    float depth_;
private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    SG2D( const SG2D& old ); // disallow copy constructor
};

class SG3D : public SGND
{
public:
    SG3D( int rcnm, int rcid )
        : SGND( rcnm, rcid ),
          z_( NULL )
    {}
    virtual ~SG3D()
    {
        setLength( 0 );
    }
    virtual void setLength( int length )
    {
        if( length_ > 0 && NULL != z_ )
        {
            delete[] z_;
            z_ = NULL;
        }
        SGND::setLength( length );
        if( length > 0 )
        {
            z_ = new int[length];
            for( int i = 0; i < length; ++i )
            {
                z_[i] = 0x7FFFFFFF;
            }
        }
    }
    virtual void set( int index, int x, int y, int z )
    {
        SGND::set( index, x, y );
        if( index < length_ )
        {
            z_[index] = z;
        }
    }
    int getZ( int index )
    {
        return index < length_ ? z_[reverse_ ? length_ - 1 - index : index] : -1;
    }
    SG3D* getNext()
    {
        return ( SG3D* )next_;
    }
    static SG3D* Find( SG3D* start, int rcnm, int rcid )
    {
        return ( SG3D* )SGND::Find( start, rcnm, rcid );
    }
private:
    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    SG3D( const SG3D& old ); // disallow copy constructor
    int* z_;
};

int main( int argc, char **argv )
{

    if( argc == 1 )
    {
        show_usage();
        return 1;
    }

    const char* filename = NULL;
    SG2D** lines2D = NULL;
    int numLines2D = 0;
    bool verbose( false );
    bool quiet( false );
    bool dump( false );

    if( argc == 2 )
    {
        filename = argv[1];
    }

    if( argc > 2 )
    {
        for( int i = 1; i < argc; ++i )
        {
            if( *( argv[i] ) == '-' )
            {
                if( argv[i][1] == 'v' )
                {
                    verbose = true;
                }
                else if( argv[i][1] == 'q' )
                {
                    quiet = true;
                }
                else if( argv[i][1] == 'd' )
                {
                    dump = true;
                }
            }
            else
            {
                filename = argv[i];
            }
        }
    }

    if( NULL == filename )
    {
        show_usage();
        return 1;
    }

    SG2D* start2d( NULL );
    SG2D* end2d( NULL );

    SG3D* start3d( NULL );
    SG3D* end3d( NULL );

    CPLSetConfigOption( "CPL_DEBUG", "OFF" );
    S57Reader* reader = new S57Reader( filename );
    if( !reader->Open( true ) )
    {
        printf( "Error opening file.\n" );
        return 1;
    }

    //reader->Ingest();
    DDFModule* module = reader->GetModule();
    DDFRecord* record;

    // 1st pass, get 2D coastlines (OBJL=30) and 3D soundings (OBJL=129)
    while( ( record = module->ReadRecord() ) != NULL )
    {
        if( verbose )
        {
            for( int i = 0;  i < record->GetFieldCount(); ++i )
            {
                DDFField* field = record->GetField( i );
                DDFFieldDefn* fieldDefn = field->GetFieldDefn();
                printf( "%s(%d* ", fieldDefn->GetName(), field->GetRepeatCount() );
                for( int j = 0; j < field->GetRepeatCount(); ++j )
                {
                    for( int k = 0; k < fieldDefn->GetSubfieldCount(); ++k )
                    {
                        DDFSubfieldDefn* subFieldDefn = fieldDefn->GetSubfield( k );
                        printf( "%s[", subFieldDefn->GetName() );
                        int bytesRemaining;
                        const char* data = field->GetSubfieldData( subFieldDefn, &bytesRemaining, j );
                        int bytesRead;
                        double mult = 1.0;
                        const char* intFmt = "%.0f] ";
                        switch( subFieldDefn->GetType() )
                        {
                        case DDFInt:
                            if( *( subFieldDefn->GetName() + 1 ) == 'C' && *( subFieldDefn->GetName() + 2 ) == 'O' && *( subFieldDefn->GetName() + 3 ) == 'O' )
                            {
                                mult = 1e-7;
                                intFmt = "%.7f] ";
                            }
                            else if( *( subFieldDefn->GetName() + 0 ) == 'V' && *( subFieldDefn->GetName() + 1 ) == 'E' && *( subFieldDefn->GetName() + 2 ) == '3' )
                            {
                                mult = 1e-1;
                                intFmt = "%.1f] ";
                            }
                            printf( intFmt, mult * subFieldDefn->ExtractIntData( data, bytesRemaining, &bytesRead ) );
                            break;
                        case DDFFloat:
                            printf( "%g] ", subFieldDefn->ExtractFloatData( data, bytesRemaining, &bytesRead ) );
                            break;
                        case DDFString:
                            printf( "%s] ", subFieldDefn->ExtractStringData( data, bytesRemaining, &bytesRead ) );
                            break;
                        case DDFBinaryString:
                            if( strncmp( subFieldDefn->GetName(), "NAME", 5 ) == 0 )
                            {
                                unsigned char * uData = ( unsigned char * )data;
                                int rcnm = uData[0];
                                int rcid = uData[1] + ( uData[2] << 8 )
                                           + ( uData[3] << 16 ) + ( uData[4] << 24 );
                                printf( "RCNM[%d] RCID[%d] ] ", rcnm, rcid );
                            }
                            else
                            {
                                printf( "<BIN>] " );
                            }
                            break;
                        }
                    }
                }
                printf( ") " );
            }
            printf( "\n" );
        }

        // Deal with Values
        if( 0 == strncmp( record->GetField( 1 )->GetFieldDefn()->GetName(), "VRID", 5 ) )
        {
            int rcnm = record->GetIntSubfield( "VRID", 0, "RCNM", 0 );
            int rcid = record->GetIntSubfield( "VRID", 0, "RCID", 0 );

            // Deal with 2d stuff...
            DDFField* field2D = record->FindField( "SG2D" );
            // only deal with SG2D Connected Nodes
            if( rcnm == 120 || rcnm == 130 )
            {
                SG2D* sg2d = new SG2D( rcnm, rcid );
                if( NULL == start2d )
                {
                    start2d = sg2d;
                }
                if( NULL != end2d )
                {
                    end2d->setNext( sg2d );
                }
                end2d = sg2d;

                DDFField* vField = record->FindField( "VRPT" );
                int extra2d = 0;
                if( NULL != vField )
                {
                    extra2d = vField->GetRepeatCount();
                    extra2d = extra2d > 2 ? 2 : extra2d;
                    DDFFieldDefn* vFieldDefn = vField->GetFieldDefn();
                    DDFSubfieldDefn* vNameSubfieldDefn = vFieldDefn->FindSubfieldDefn( "NAME" );
                    if( NULL != vNameSubfieldDefn && extra2d > 0 )
                    {
                        unsigned char* nameData = ( unsigned char * )vField->GetSubfieldData(
                                                      vNameSubfieldDefn, NULL, 0 );
                        rcnm = nameData[0];
                        rcid = nameData[1] + ( nameData[2] << 8 )
                               + ( nameData[3] << 16 ) + ( nameData[4] << 24 );
                        sg2d->setStart( rcnm, rcid );
                    }
                    if( NULL != vNameSubfieldDefn && extra2d > 1 )
                    {
                        unsigned char* nameData = ( unsigned char * )vField->GetSubfieldData(
                                                      vNameSubfieldDefn, NULL, 1 );
                        rcnm = nameData[0];
                        rcid = nameData[1] + ( nameData[2] << 8 )
                               + ( nameData[3] << 16 ) + ( nameData[4] << 24 );
                        sg2d->setEnd( rcnm, rcid );
                    }
                }
                int offset2d = extra2d > 0 ? 1 : 0;
                if( NULL == field2D )
                {
                    sg2d->setLength( extra2d );
                }
                else
                {
                    DDFFieldDefn* fieldDefn = field2D->GetFieldDefn();
                    DDFSubfieldDefn* xSubDefn = fieldDefn->FindSubfieldDefn( "XCOO" );
                    DDFSubfieldDefn* ySubDefn = fieldDefn->FindSubfieldDefn( "YCOO" );
                    int bytesRemaining;
                    int bytesRead;
                    int count = field2D->GetRepeatCount();
                    sg2d->setLength( count + extra2d );
                    for( int i = 0; i < count; ++i )
                    {
                        const char* data = field2D->GetSubfieldData( xSubDefn, &bytesRemaining, i );
                        int x = xSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
                        data = field2D->GetSubfieldData( ySubDefn, &bytesRemaining, i );
                        int y = ySubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
                        sg2d->set( i + offset2d, x, y );
                    }
                }
            }

            // Deal with 3d stuff
            DDFField* field3d = record->FindField( "SG3D" );
            if( NULL != field3d )
            {
                SG3D* sg3d = new SG3D( rcnm, rcid );
                if( NULL == start3d )
                {
                    start3d = sg3d;
                }
                if( NULL != end3d )
                {
                    end3d->setNext( sg3d );
                }
                end3d = sg3d;

                DDFFieldDefn* fieldDefn = field3d->GetFieldDefn();
                DDFSubfieldDefn* xSubDefn = fieldDefn->FindSubfieldDefn( "XCOO" );
                DDFSubfieldDefn* ySubDefn = fieldDefn->FindSubfieldDefn( "YCOO" );
                DDFSubfieldDefn* zSubDefn = fieldDefn->FindSubfieldDefn( "VE3D" );
                int bytesRemaining;
                int bytesRead;
                sg3d->setLength( field3d->GetRepeatCount() );
                for( int i = 0; NULL != field3d && i < sg3d->getLength(); ++i )
                {
                    const char* data = field3d->GetSubfieldData( xSubDefn, &bytesRemaining, i );
                    int x = xSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
                    data = field3d->GetSubfieldData( ySubDefn, &bytesRemaining, i );
                    int y = ySubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
                    data = field3d->GetSubfieldData( zSubDefn, &bytesRemaining, i );
                    int z = zSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
                    sg3d->set( i, x, y, z );
                }
            }
        }

        // Deal with Features
        if( 0 == strncmp( record->GetField( 1 )->GetFieldDefn()->GetName(), "FRID", 5 ) )
        {
            // We only care about coastlines and soundings
            int objl = record->GetIntSubfield( "FRID", 0, "OBJL", 0 );
            if( objl != 302 ) //&& objl != 30 && objl != 43 && objl != 129 )
            {
                continue;
            }

            DDFField* field = record->FindField( "FSPT" );
            if( objl == 302 )
            {
                int covType = record->GetIntSubfield( "ATTF", 0, "ATVL", 0 );
                if( covType == 1 )
                {
                    numLines2D =  field->GetRepeatCount();
                    lines2D = new SG2D*[numLines2D];
                }
                else
                {
                    continue;
                }
            }

            for( int i = 0; NULL != field && i < field->GetRepeatCount(); ++i )
            {
                unsigned char* nameData = ( unsigned char * )
                                          field->GetSubfieldData(
                                              field->GetFieldDefn()->FindSubfieldDefn( "NAME" ),
                                              NULL, i );
                int rcnm = nameData[0];
                int rcid = nameData[1] + ( nameData[2] << 8 )
                           + ( nameData[3] << 16 ) + ( nameData[4] << 24 );

                SG2D* sg2d = NULL;
                float depth( 0.0f );
                if( objl == 302 )  // coverage
                {
                    sg2d = SG2D::Find( start2d, rcnm, rcid );
                    lines2D[i] = sg2d;
                    int ornt = record->GetIntSubfield( "FSPT", 0, "ORNT", i );
                    //printf("For %d, ornt = %d\n", i, ornt);
                    if( NULL != sg2d )
                    {
                        sg2d->setReverse( ornt == 2 );
                    }
                }
                if( objl == 30 )  // coastline
                {
                    sg2d = SG2D::Find( start2d, rcnm, rcid );
                }
                if( objl == 43 )  // depth
                {
                    depth = record->GetFloatSubfield( "ATTF", 0, "ATVL", 3 );
                    sg2d = SG2D::Find( start2d, rcnm, rcid );

                }
                else if( objl == 129 )  // sounding
                {
                    /*
                    SG3D* sg3d = SG3D::Find( start3d, rcnm, rcid );
                    if ( NULL != sg3d )
                    {
                        sg3d->setUsed( true );
                    }
                    */
                }
                if( NULL != sg2d )
                {
                    sg2d->setUsed( true );
                    if( !isnan( sg2d->getDepth() ) )
                    {
                        printf( "********** duplicate depth: %g and %g\n", sg2d->getDepth(), depth );
                    }
                    sg2d->setDepth( depth );
                    // Deal with start & end
                    SG2D* sg2dStartEnd = NULL;
                    if( sg2d->getStart( rcnm, rcid ) &&
                            NULL != ( sg2dStartEnd = SG2D::Find( start2d, rcnm, rcid ) ) )
                    {
                        sg2d->set( 0, sg2dStartEnd->getX( 0 ), sg2dStartEnd->getY( 0 ) );
                    }
                    if( sg2d->getEnd( rcnm, rcid ) &&
                            NULL != ( sg2dStartEnd = SG2D::Find( start2d, rcnm, rcid ) ) )
                    {
                        sg2d->set( sg2d->getLength() - 1, sg2dStartEnd->getX( 0 ), sg2dStartEnd->getY( 0 ) );
                    }
                    if( dump )
                    {
                        for( int j = 0; j < sg2d->getLength(); ++j )
                        {
                            printf( "%11.6f,%10.6f,%7.1f;", sg2d->getX( j ) * 1e-7, sg2d->getY( j ) * 1e-7, sg2d->getDepth() );
                        }
                    }
                }
                else if( dump )
                {
                    printf( "null" );
                }
                if( dump )
                {
                    //printf("\n");
                }
            }
        }
    }

    if( dump )
    {
        printf( "\n" );
    }

    if( !quiet )
    {
        // Display 2d stuff..
        SG2D* sg2d = start2d;
        int num2d = 0;
        printf( "2D:\n" );
        while( sg2d != NULL )
        {
            if( sg2d ->isUsed() )
            {
                for( int j = 0; j < sg2d->getLength(); ++j )
                {
                    printf( "%11.6f,%10.6f,%7.1f;", sg2d->getX( j ) * 1e-7, sg2d->getY( j ) * 1e-7, sg2d->getDepth() );
                }
                num2d += sg2d->getLength();
            }
            sg2d = sg2d->getNext();
        }
        printf( "\nnum2d=%d\n", num2d );
        // Display 3d stuff...
        SG3D* sg3d = start3d;
        int num3d = 0;
        printf( "3D:\n" );
        while( sg3d != NULL )
        {
            if( sg3d ->isUsed() )
            {
                for( int j = 0; j < sg3d->getLength(); ++j )
                {
                    printf( "%.7f,%.7f,%.1f;", sg3d->getX( j ) * 1e-7, sg3d->getY( j ) * 1e-7, sg3d->getZ( j ) * 1e-1 );
                }
                num3d += sg3d->getLength();
            }
            sg3d = sg3d->getNext();
        }
        printf( "\nnum3d=%d\n", num3d );
        for( int i = 0; i < numLines2D; ++i )
        {
            if( NULL == lines2D[i] )
            {
                continue;
            }
            sg2d = lines2D[i];
            lines2D[i] = NULL;
            double startX = sg2d->getX( 0 );
            double startY = sg2d->getY( 0 );
            double prevX = startX;
            double prevY = startY;
            double lastX = startX;
            double lastY = startY;
            double endX = 0;
            double endY = 0;
            printf( "%10.5f,%9.5f;", startX * 1e-7, startY * 1e-7 );
            for( int j = 0; j < sg2d->getLength(); ++j )
            {
                endX = sg2d->getX( j );
                endY = sg2d->getY( j );
                if( ( prevX == lastX && prevX == endX )
                        || ( prevX == lastY && prevY == endY ) )
                {
                    //printf( "/nskip %10.5f,%9.5f;\n", lastX * 1e-7, lastY * 1e-7 );
                }
                else
                {
                    double diff = fabs( lastY - ( prevY + ( endY - prevY ) / ( endX - prevX ) * ( lastX - prevX ) ) );
                    if( diff > 1500 )
                    {
                        printf( "%10.5f,%9.5f;", lastX * 1e-7, lastY * 1e-7 );
                        prevX = lastX;
                        prevY = lastY;
                    }
                }
                lastX = endX;
                lastY = endY;
            }
            for( int k = 0; k < numLines2D; ++k )
            {
                if( NULL == lines2D[k] )
                {
                    continue;
                }
                sg2d = lines2D[k];
                if( endX == sg2d->getX( 0 ) && endY == sg2d->getY( 0 ) )
                {
                    //printf("\n$link to %d\n", k);
                    for( int j = 1; j < sg2d->getLength(); ++j )
                    {
                        endX = sg2d->getX( j );
                        endY = sg2d->getY( j );
                        if( ( prevX == lastX && prevX == endX )
                                || ( prevY == lastY && prevY == endY ) )
                        {

                        }
                        else
                        {
                            double diff = fabs( lastY - ( prevY + ( endY - prevY ) / ( endX - prevX ) * ( lastX - prevX ) ) );
                            if( diff > 1500 )
                            {
                                printf( "%10.5f,%9.5f;", lastX * 1e-7, lastY * 1e-7 );
                                prevX = lastX;
                                prevY = lastY;
                            }
                        }
                        lastX = endX;
                        lastY = endY;
                    }
                    lines2D[k] = NULL;
                    k = 0;
                }
                else
                {
                    //printf("%d!=%d and %d!=%d\n",endX,sg2d->getX(0),endY,sg2d->getY(0));
                }
            }
            if( endX == startX && endY == startY )
            {
                printf( "%10.5f,%9.5f;", endX * 1e-7, endY * 1e-7 );
                printf( "---" );
            }
            printf( "\n" );
        }
    }

    if( NULL != start2d )
    {
        delete start2d;
    }
    if( NULL != start3d )
    {
        delete start3d;
    }

    reader->Close();
    delete reader;

    CPLFreeConfig();

    return 0;
}