/*
 * EncReader.cpp
 *
 *  Created on: Apr 5, 2011
 *      Author: godin
 */

#include "EncReader.h"

#include "utils/AuvMath.h"

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

EncReader::EncReader( const char* filename )
    : filename_( filename ),
      contours_( true ),
      coverages_( true ),
      soundings_( true ),
      scanStep_( SCAN_SETUP ),
      s57Reader_( NULL ),
      ddfModule_( NULL ),
      record_( NULL ),
      repeatRecord_( false )
{}

EncReader::~EncReader()
{
}

/// Returns false until initialization is done.
bool EncReader::scan( EncScanPoints& points, Logger& logger )
{
    switch( scanStep_ )
    {
    case SCAN_SETUP:
        logger.syslog( "Setup scan of " + filename_, Syslog::INFO );
        CPLSetConfigOption( "CPL_DEBUG", "OFF" );
        s57Reader_ = new S57Reader( filename_.cStr() );

        if( !s57Reader_->Open( true ) )
        {
            logger.syslog( "Error opening file: " + filename_, Syslog::FAULT );
            delete s57Reader_;
            scanStep_ = SCAN_DONE;
            break;
        }

        ddfModule_ = s57Reader_->GetModule();

        scanStep_ = SCAN_FEATURES;

    // Intentionally no break here.
    // First pass...
    case SCAN_FEATURES:
        record_ = ddfModule_->ReadRecord();
        if( NULL == record_ )
        {
            scanStep_ = SCAN_FEATURES_DONE;
            break;
        }

        // Deal only with Features
        if( 0 == strncmp( record_->GetField( 1 )->GetFieldDefn()->GetName(), "FRID", 5 ) )
        {
            // We only care about coastlines (30), depth contours (43), soundings (129), and coverage (302)
            int objl = record_->GetIntSubfield( "FRID", 0, "OBJL", 0 );
            if( objl != 30 && objl != 43 && objl != 129 && objl != 302 )
            {
                break;
            }
            // Only care about inclusive coverage
            if( objl == 302 )
            {
                int covType = record_->GetIntSubfield( "ATTF", 0, "ATVL", 0 );
                if( covType != 1 )
                {
                    break;
                }
            }

            float depth( 0.0f );
            // Contours have one depth
            if( objl == 43 )
            {
                DDFField* attfField = record_->FindField( "ATTF" );
                for( int j = 0; j < attfField->GetRepeatCount(); ++j )
                {
                    if( record_->GetIntSubfield( "ATTF", 0, "ATTL", j ) == 174 )
                    {
                        depth = record_->GetFloatSubfield( "ATTF", 0, "ATVL", j );
                    }
                }
            }

            // Ok, let's consider the fields.
            DDFField* fsptField = record_->FindField( "FSPT" );
            if( NULL != fsptField )
            {
                DDFSubfieldDefn* nameFieldDefn = fsptField->GetFieldDefn()->FindSubfieldDefn( "NAME" );

                for( int i = 0; i < fsptField->GetRepeatCount(); ++i )
                {
                    unsigned char* nameData = ( unsigned char * )
                                              fsptField->GetSubfieldData( nameFieldDefn,
                                                      NULL, i );
                    unsigned int rcnm = nameData[0];
                    unsigned int rcid = nameData[1] + ( nameData[2] << 8 )
                                        + ( nameData[3] << 16 ) + ( nameData[4] << 24 );
                    unsigned int id = ( rcnm << 24 ) + rcid;

                    if( objl == 30 || objl == 43 )  // coastline or contour
                    {
                        EncContour* contour;
                        if( i == 0 )
                        {
                            contour = new EncContour();
                            contour->depth_ = depth;
                            contour->length_ = fsptField->GetRepeatCount();
                            contour->edges_ = new EncEdge*[contour->length_];
                            contours_.push( contour );
                        }
                        else
                        {
                            contour = contours_.peek();
                        }
                        EncEdge* edge = new EncEdge();
                        edge->id_ = id;
                        contour->edges_[i] = edge;
                    }
                    else if( objl == 129 )  // sounding
                    {
                        EncSounding* sounding = new EncSounding();
                        soundings_.push( sounding );
                        sounding->soundingId_ = id;
                    }
                    else if( objl == 302 )  // coverage
                    {
                        EncArea* area;
                        if( i == 0 )
                        {
                            area = new EncArea();
                            area->length_ = fsptField->GetRepeatCount();
                            area->edges_ = new EncEdge*[area->length_];
                            area->reverses_ = new bool[area->length_];
                            coverages_.push( area );
                        }
                        else
                        {
                            area = coverages_.peek();
                        }
                        EncEdge* edge = new EncEdge();
                        edge->id_ = id;
                        area->edges_[i] = edge;
                        int ornt = record_->GetIntSubfield( "FSPT", 0, "ORNT", i );
                        area->reverses_[i] = ornt == 2;
                    }
                }
            }
        }
        break;

    case SCAN_FEATURES_DONE:
        logger.syslog( "Done scanning features of " + filename_, Syslog::INFO );
        s57Reader_->Close();
        s57Reader_->Open( false );
        ddfModule_ = s57Reader_->GetModule();
        scanStep_ = SCAN_EDGES;

    // Intentionally no break here.
    // Second pass...
    case SCAN_EDGES:
        record_ = ddfModule_->ReadRecord();
        if( NULL == record_ )
        {
            scanStep_ = SCAN_EDGES_DONE;
            break;
        }

        if( 0 == strncmp( record_->GetField( 1 )->GetFieldDefn()->GetName(), "VRID", 5 ) )
        {
            unsigned int rcnm = record_->GetIntSubfield( "VRID", 0, "RCNM", 0 );
            // Only deal with Edges
            if( rcnm == 110 || rcnm == 120 )
            {
                break;
            }
            // Bail when we are done with Edges
            if( rcnm == 140 || rcnm == 100 )
            {
                scanStep_ = SCAN_EDGES_DONE;
                break;
            }
            unsigned int rcid = record_->GetIntSubfield( "VRID", 0, "RCID", 0 );
            unsigned int fieldId = ( rcnm << 24 ) + rcid;

            // Only deal with needed Edges
            for( unsigned int i = 0; i < contours_.size(); ++i )
            {
                EncContour* contour = contours_.get( i );
                if( contour->edgeCount_ == contour->length_ )
                {
                    continue;
                }
                for( int j = 0; j < contour->length_; ++j )
                {
                    EncEdge* edge = contour->edges_[j];
                    if( fieldId == edge->id_ )
                    {
                        populateEdge( edge, record_, false );
                        if( edge->length_ > 0 )
                        {
                            points.length_ = edge->length_;
                            points.x_ = edge->x_;
                            points.y_ = edge->y_;
                            points.zz_ = contour->depth_;
                            edge->length_ = 0;
                            edge->x_ = NULL;
                            edge->y_ = NULL;
                            ++contour->edgeCount_;
                            if( contour->edgeCount_ == contour->length_ )
                            {
                                reduceNodes( contour );
                            }
                        }
                    }
                }
            }
            for( unsigned int i = 0; i < coverages_.size(); ++i )
            {
                EncArea* area = coverages_.get( i );
                if( area->edgeCount_ == area->length_ )
                {
                    continue;
                }
                for( int j = 0; j < area->length_; ++j )
                {
                    EncEdge* edge = area->edges_[j];
                    if( fieldId == edge->id_ )
                    {
                        populateEdge( edge, record_, area->reverses_[j] );
                        ++area->edgeCount_;
                        if( area->edgeCount_ == area->length_ )
                        {
                            logger.syslog( "Calculating coverage of " + filename_, Syslog::INFO );
                            sortNodes( area );
                        }
                    }
                }
            }
        }
        break;

    case SCAN_EDGES_DONE:
        logger.syslog( "Done scanning edges of " + filename_, Syslog::INFO );
        s57Reader_->Close();
        s57Reader_->Open( false );
        ddfModule_ = s57Reader_->GetModule();
        scanStep_ = SCAN_NODES;

        for( unsigned int i = 0; i < contours_.size(); ++i )
        {
            contours_.get( i ) ->edgeCount_ = 0;
        }
        for( unsigned int i = 0; i < coverages_.size(); ++i )
        {
            coverages_.get( i ) ->edgeCount_ = 0;
        }

    // Intentionally no break here.
    // Third pass...
    case SCAN_NODES:
        if( repeatRecord_ )
        {
            repeatRecord_ = false;
        }
        else
        {
            record_ = ddfModule_->ReadRecord();
        }
        if( NULL == record_ )
        {
            scanStep_ = SCAN_NODES_DONE;
            break;
        }

        if( 0 == strncmp( record_->GetField( 1 )->GetFieldDefn()->GetName(), "VRID", 5 ) )
        {
            unsigned int rcnm = record_->GetIntSubfield( "VRID", 0, "RCNM", 0 );
            // Only deal with Nodes
            if( rcnm != 110 && rcnm != 120 )
            {
                scanStep_ = SCAN_NODES_DONE;
                break;
            }

            unsigned int rcid = record_->GetIntSubfield( "VRID", 0, "RCID", 0 );
            unsigned int fieldId = ( rcnm << 24 ) + rcid;

            // Only deal with needed Nodes, in this case soundings
            for( unsigned int i = 0; rcnm == 110 && i < soundings_.size(); ++i )
            {
                EncSounding* sounding = soundings_.get( i );
                if( sounding->soundingId_ == fieldId )
                {
                    populatePoints( points, record_ );
                }
            }

            // Only deal with needed Nodes, in this case coutour edges
            for( unsigned int i = 0; rcnm == 120 && !repeatRecord_ && i < contours_.size(); ++i )
            {
                EncContour* contour = contours_.get( i );
                if( contour->edgeCount_ == contour->length_ )
                {
                    continue;
                }
                for( int j = 0; j < contour->length_; ++j )
                {
                    EncEdge* edge = contour->edges_[j];
                    bool edgeDone( false );
                    if( fieldId == edge->startNode_.id_ )
                    {
                        populateNode( edge->startNode_, record_ );
                        edgeDone =  edge->endNode_.id_ == 0 ||
                                    edge->endNode_.x_ != 0 || edge->endNode_.y_ != 0;

                    }
                    if( fieldId == edge->endNode_.id_ )
                    {
                        populateNode( edge->endNode_, record_ );
                        edgeDone = edge->startNode_.id_ == 0 ||
                                   edge->startNode_.x_ != 0 || edge->startNode_.y_ != 0;
                    }
                    if( edgeDone )
                    {
                        if( points.length_ > 0 )
                        {
                            repeatRecord_ = true;
                            break;
                        }
                        ++contour->edgeCount_;
                        if( contour->edgeCount_ == contour->length_ )
                        {
                            int nodeCount = 0;
                            for( int k = 0; k < contour->length_; ++k )
                            {
                                edge = contour->edges_[k];
                                nodeCount += edge->startNode_.id_ == 0 ? 0 : 1;
                                nodeCount += edge->endNode_.id_ == 0 ? 0 : 1;
                            }
                            int* x = new int[nodeCount];
                            int* y = new int[nodeCount];
                            int nodeIndex = 0;
                            for( int k = 0; k < contour->length_; ++k )
                            {
                                edge = contour->edges_[k];
                                if( edge->startNode_.id_ != 0 )
                                {
                                    x[nodeIndex] = edge->startNode_.x_;
                                    y[nodeIndex] = edge->startNode_.y_;
                                    ++nodeIndex;
                                }
                                if( edge->endNode_.id_ != 0 )
                                {
                                    x[nodeIndex] = edge->endNode_.x_;
                                    y[nodeIndex] = edge->endNode_.y_;
                                    ++nodeIndex;
                                }
                            }
                            points.length_ = nodeCount;
                            points.x_ = x;
                            points.y_ = y;
                            points.zz_ = contour->depth_;
                            break;
                        }
                    }
                }
            }

            // Only deal with needed Nodes, in this case coutour edges
            for( unsigned int i = 0; rcnm == 120 && i < coverages_.size(); ++i )
            {
                EncArea* area = coverages_.get( i );
                if( area->edgeCount_ == area->length_ )
                {
                    continue;
                }
                for( int j = 0; j < area->length_; ++j )
                {
                    EncEdge* edge = area->edges_[j];
                    bool edgeDone( false );
                    if( fieldId == edge->startNode_.id_ )
                    {
                        populateNode( edge->startNode_, record_ );
                        edgeDone =  edge->endNode_.id_ == 0 ||
                                    edge->endNode_.x_ != 0 || edge->endNode_.y_ != 0;

                    }
                    if( fieldId == edge->endNode_.id_ )
                    {
                        populateNode( edge->endNode_, record_ );
                        edgeDone = edge->startNode_.id_ == 0 ||
                                   edge->startNode_.x_ != 0 || edge->startNode_.y_ != 0;
                    }
                    if( edgeDone )
                    {
                        ++area->edgeCount_;
                        if( area->edgeCount_ == area->length_ )
                        {
                            simplifyArea( area );
                        }
                    }
                }
            }

        }
        break;

    case SCAN_NODES_DONE:
        logger.syslog( "Done scanning nodes of " + filename_, Syslog::INFO );
        s57Reader_->Close();
        scanStep_ = SCAN_DONE;

    // Intentionally no break here
    case SCAN_DONE:
        delete s57Reader_;
        CPLFreeConfig();
        return true;
    }

    return false;
}

EncArea* EncReader::popCoverage()
{
    return coverages_.pop();
}

void EncReader::uninitialize()
{

}

void EncReader::populateEdge( EncEdge* edge, DDFRecord* record, const bool& reverse )
{
    DDFField* vField = record->FindField( "VRPT" );
    if( NULL != vField )
    {
        DDFFieldDefn* vFieldDefn = vField->GetFieldDefn();
        DDFSubfieldDefn* vNameSubfieldDefn = vFieldDefn->FindSubfieldDefn( "NAME" );
        if( NULL != vNameSubfieldDefn )
        {
            unsigned char* nameData = ( unsigned char * )vField->GetSubfieldData(
                                          vNameSubfieldDefn, NULL, reverse ? 1 : 0 );
            edge->startNode_.id_ = ( nameData[0] << 24 ) +
                                   nameData[1] + ( nameData[2] << 8 )
                                   + ( nameData[3] << 16 ) + ( nameData[4] << 24 );

            nameData = ( unsigned char * )vField->GetSubfieldData(
                           vNameSubfieldDefn, NULL, reverse ? 0 : 1 );
            edge->endNode_.id_ = ( nameData[0] << 24 ) +
                                 nameData[1] + ( nameData[2] << 8 )
                                 + ( nameData[3] << 16 ) + ( nameData[4] << 24 );
        }
    }
    DDFField* field2D = record->FindField( "SG2D" );
    if( NULL != field2D )
    {
        DDFFieldDefn* fieldDefn = field2D->GetFieldDefn();
        DDFSubfieldDefn* xSubDefn = fieldDefn->FindSubfieldDefn( "XCOO" );
        DDFSubfieldDefn* ySubDefn = fieldDefn->FindSubfieldDefn( "YCOO" );
        int bytesRemaining;
        int bytesRead;
        edge->length_ = field2D->GetRepeatCount();
        edge->x_ = new int[edge->length_];
        edge->y_ = new int[edge->length_];
        for( int i = 0; i < edge->length_; ++i )
        {
            const char* data = field2D->GetSubfieldData( xSubDefn, &bytesRemaining, i );
            edge->x_[reverse ? edge->length_ - 1 - i : i] =
                xSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
            data = field2D->GetSubfieldData( ySubDefn, &bytesRemaining, i );
            edge->y_[reverse ? edge->length_ - 1 - i : i] =
                ySubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
        }

    }
}

void EncReader::populateNode( EncNode& node, DDFRecord* record )
{
    DDFField* field2D = record->FindField( "SG2D" );
    if( NULL != field2D )
    {
        DDFFieldDefn* fieldDefn = field2D->GetFieldDefn();
        DDFSubfieldDefn* xSubDefn = fieldDefn->FindSubfieldDefn( "XCOO" );
        DDFSubfieldDefn* ySubDefn = fieldDefn->FindSubfieldDefn( "YCOO" );
        int bytesRemaining;
        int bytesRead;
        const char* data = field2D->GetSubfieldData( xSubDefn, &bytesRemaining, 0 );
        node.x_ =
            xSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
        data = field2D->GetSubfieldData( ySubDefn, &bytesRemaining, 0 );
        node.y_ =
            ySubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
    }

}

void EncReader::populatePoints( EncScanPoints& points, DDFRecord* record )
{
    DDFField* field3d = record->FindField( "SG3D" );
    if( NULL != field3d )
    {

        DDFFieldDefn* fieldDefn = field3d->GetFieldDefn();
        DDFSubfieldDefn* xSubDefn = fieldDefn->FindSubfieldDefn( "XCOO" );
        DDFSubfieldDefn* ySubDefn = fieldDefn->FindSubfieldDefn( "YCOO" );
        DDFSubfieldDefn* zSubDefn = fieldDefn->FindSubfieldDefn( "VE3D" );
        int bytesRemaining;
        int bytesRead;
        points.length_ = field3d->GetRepeatCount();
        points.x_ = new int[points.length_];
        points.y_ = new int[points.length_];
        points.z_ = new int[points.length_];
        for( int i = 0; NULL != field3d && i < points.length_; ++i )
        {
            const char* data = field3d->GetSubfieldData( xSubDefn, &bytesRemaining, i );
            points.x_[i] = xSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
            data = field3d->GetSubfieldData( ySubDefn, &bytesRemaining, i );
            points.y_[i]  = ySubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
            data = field3d->GetSubfieldData( zSubDefn, &bytesRemaining, i );
            points.z_[i]  = zSubDefn->ExtractIntData( data, bytesRemaining, &bytesRead );
        }
    }
}

void EncReader::reduceNodes( EncContour* contour )
{

}

void EncReader::sortNodes( EncArea* area )
{
    for( int i = 0; i < area->length_; ++i )
    {
        unsigned int endId = area->edges_[i]->endNode_.id_;
        for( int j = i + 1; j < area->length_; ++j )
        {
            EncEdge* edge = area->edges_[j];
            if( edge->startNode_.id_ == endId )
            {
                if( j != i + 1 )
                {
                    EncEdge* tmpEdge = area->edges_[i + 1];
                    area->edges_[i + 1] = edge;
                    area->edges_[j] = tmpEdge;
                }
                endId = edge->endNode_.id_;
                ++i;
                j = i + 1;
            }
        }
    }

}

void EncReader::simplifyArea( EncArea* area )
{
    area->segments_ = 0;
    for( int i = 0; i < area->length_; ++i )
    {
        EncEdge* edge = area->edges_[i];
        area->segments_ += edge->length_;
        ++area->segments_;
    }

    area->x0_ = new float[area->segments_];
    area->y0_ = new float[area->segments_];
    area->x1_ = new float[area->segments_];
    area->y1_ = new float[area->segments_];
    for( int i = 0; i < area->segments_; ++i )
    {
        area->x0_[i] = area->x1_[i] = area->y0_[i] = area->y1_[i] = -123.456789;
    }
    int segIndex = 0;
    int shapeIndex = 0;
    unsigned int startId = 0;
    float lastX( nanf( "" ) ), lastY( nanf( "" ) );
    float prevX( nanf( "" ) ), prevY( nanf( "" ) );
    float endX( nanf( "" ) ), endY( nanf( "" ) );
    for( int i = 0; i < area->length_; ++i )
    {
        EncEdge* edge = area->edges_[i];
        if( 0 == startId )
        {
            startId = edge->startNode_.id_;
            lastX = edge->startNode_.x_;
            lastY = edge->startNode_.y_;
            prevX = nanf( "" );
            prevY = nanf( "" );
            shapeIndex = 0;
            //printf( "\nstart\n" );
        }
        for( int j = -1; j <= edge->length_; ++j )
        {
            if( j == -1 )
            {
                endX = edge->startNode_.x_;
                endY = edge->startNode_.y_;
            }
            else if( j == edge->length_ )
            {
                endX = edge->endNode_.x_;
                endY = edge->endNode_.y_;
            }
            else
            {
                endX = edge->x_[j];
                endY = edge->y_[j];
            }
            float diff( 1e10 );
            if( ( prevX == lastX && prevX == endX )
                    || ( prevY == lastY && prevY == endY ) )
            {
                diff = 0;
            }
            else   if( !isnan( prevX ) && endX != prevX )
            {
                diff = fabs( lastY - ( prevY + ( endY - prevY ) / ( endX - prevX ) * ( lastX - prevX ) ) );
            }
            else if( !isnan( prevY ) && endY != prevY )
            {
                diff = fabs( lastX - ( prevX + ( endX - prevX ) / ( endY - prevY ) * ( lastY - prevY ) ) );
            }
            if( diff > 1500 )
            {
                //printf( "%10.5f,%9.5f;", lastX * 1e-7, lastY * 1e-7 );
                if( shapeIndex > 0 )
                {
                    area->x1_[segIndex - 1] = D2R( 1e-7 * lastX );
                    area->y1_[segIndex - 1] = D2R( 1e-7 * lastY );
                }
                area->x0_[segIndex] = D2R( 1e-7 * lastX );
                area->y0_[segIndex] = D2R( 1e-7 * lastY );
                prevX = lastX;
                prevY = lastY;
                ++segIndex;
                ++shapeIndex;
            }

            lastX = endX;
            lastY = endY;
        }
        if( edge->endNode_.id_ == startId )
        {
            //printf( "%10.5f,%9.5f;\nend\n", lastX * 1e-7, lastY * 1e-7 );
            area->x1_[segIndex - 1] = D2R( 1e-7 * endX );
            area->y1_[segIndex - 1] = D2R( 1e-7 * endY );
            startId = 0;
        }
    }
    area->segments_ = segIndex;
    /*printf("\n");
    for( int i = 0; i < area->segments_; ++i)
    {
    	printf( "%10.5f,%9.5f,%10.5f,%9.5f;", area->x0_[i] * 1e-7, area->y0_[i] * 1e-7, area->x1_[i] * 1e-7, area->y1_[i] * 1e-7 );
    }
    printf("\n");*/
    if( area->length_ > 0 )
    {
        area->edgeCount_ = 0;
        for( int i =  0 ; i < area->length_; ++i )
        {
            delete area->edges_[i];
        }
        delete[] area->edges_;
        area->length_ = 0;
        area->edges_ = NULL;
    }
}