/** \file
 *
 *  Contains the ShortestDistanceToPolygonSidesCalculator class implementation.
 *
 *  Copyright (c) 2022 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "ShortestDistanceToPolygonSidesCalculator.h"
#include "ShortestDistanceToPolygonSidesCalculatorIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

#include <cmath>

ShortestDistanceToPolygonSidesCalculator::ShortestDistanceToPolygonSidesCalculator( const Module* module )
    : SyncDerivationComponent( ShortestDistanceToPolygonSidesCalculatorIF::NAME, module ),
      verbosity_( 0 ),
      sideShortestDist_( 0 ),
      insidePolygon_( true ),
      shortestDistToPolygonSides_( nanf( "" ) )
{
    // Slate input measurements
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

    verbosityCfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::VERBOSITY );
    latVertex1CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_1 );
    lonVertex1CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_1 );
    latVertex2CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_2 );
    lonVertex2CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_2 );
    latVertex3CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_3 );
    lonVertex3CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_3 );
    latVertex4CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_4 );
    lonVertex4CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_4 );
    latVertex5CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_5 );
    lonVertex5CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_5 );
    latVertex6CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_6 );
    lonVertex6CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_6 );
    latVertex7CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_7 );
    lonVertex7CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_7 );
    latVertex8CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_8 );
    lonVertex8CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_8 );
    latVertex9CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_9 );
    lonVertex9CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_9 );
    latVertex10CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LAT_VERTEX_10 );
    lonVertex10CfgReader_ = newConfigReader( ShortestDistanceToPolygonSidesCalculatorIF::LON_VERTEX_10 );

    shortestDistToPolygonSidesWriter_ = newDataWriter( ShortestDistanceToPolygonSidesCalculatorIF::SHORTEST_DIST_TO_POLYGON_SIDES );
    sideShortestDistWriter_ = newDataWriter( ShortestDistanceToPolygonSidesCalculatorIF::SIDE_SHORTEST_DIST );
    insidePolygonWriter_ = newDataWriter( ShortestDistanceToPolygonSidesCalculatorIF::INSIDE_POLYGON );
}

ShortestDistanceToPolygonSidesCalculator::~ShortestDistanceToPolygonSidesCalculator()
{}

/// Initialize function
void ShortestDistanceToPolygonSidesCalculator::initialize( void )
{
    logger_.syslog( "Initializing.", ( verbosity_ > 0 ) ? Syslog::INFO : Syslog::DEBUG );

    readConfig();
}

void ShortestDistanceToPolygonSidesCalculator::readConfig( void )
{
    verbosityCfgReader_->read( Units::COUNT, verbosity_ );
    latVertex1CfgReader_->read( Units::RADIAN, latVertex_[0] );
    lonVertex1CfgReader_->read( Units::RADIAN, lonVertex_[0] );
    latVertex2CfgReader_->read( Units::RADIAN, latVertex_[1] );
    lonVertex2CfgReader_->read( Units::RADIAN, lonVertex_[1] );
    latVertex3CfgReader_->read( Units::RADIAN, latVertex_[2] );
    lonVertex3CfgReader_->read( Units::RADIAN, lonVertex_[2] );
    latVertex4CfgReader_->read( Units::RADIAN, latVertex_[3] );
    lonVertex4CfgReader_->read( Units::RADIAN, lonVertex_[3] );
    latVertex5CfgReader_->read( Units::RADIAN, latVertex_[4] );
    lonVertex5CfgReader_->read( Units::RADIAN, lonVertex_[4] );
    latVertex6CfgReader_->read( Units::RADIAN, latVertex_[5] );
    lonVertex6CfgReader_->read( Units::RADIAN, lonVertex_[5] );
    latVertex7CfgReader_->read( Units::RADIAN, latVertex_[6] );
    lonVertex7CfgReader_->read( Units::RADIAN, lonVertex_[6] );
    latVertex8CfgReader_->read( Units::RADIAN, latVertex_[7] );
    lonVertex8CfgReader_->read( Units::RADIAN, lonVertex_[7] );
    latVertex9CfgReader_->read( Units::RADIAN, latVertex_[8] );
    lonVertex9CfgReader_->read( Units::RADIAN, lonVertex_[8] );
    latVertex10CfgReader_->read( Units::RADIAN, latVertex_[9] );
    lonVertex10CfgReader_->read( Units::RADIAN, lonVertex_[9] );
}

void ShortestDistanceToPolygonSidesCalculator::run( void )
{
    readConfig(); // Read in latitudes and longtitudes of polygon vertices if they are modified between missions, without the need to "restart app".

    if( calculate() )
        writeData();
}

bool ShortestDistanceToPolygonSidesCalculator::calculate( void )
{
    double latitude( nanf( "" ) ), longitude( nanf( "" ) ), xVertex[MAX_NUM_VERTICES], yVertex[MAX_NUM_VERTICES], CrossProduct[MAX_NUM_VERTICES];

    double x, y, Dv1, Dv2, Xp[MAX_NUM_VERTICES], Yp[MAX_NUM_VERTICES], D[MAX_NUM_VERTICES], sideLengthSquared;
    int k, numVertices;

    if( latitudeReader_->read( Units::RADIAN, latitude ) && longitudeReader_->read( Units::RADIAN, longitude ) )
    {
        numVertices = 0;

        for( k = 0; k <= ( MAX_NUM_VERTICES - 1 ); k++ )
        {
            if( ( !isnan( latVertex_[k] ) ) && ( !isnan( lonVertex_[k] ) ) )
                numVertices++;
            else
                break;
        }

        if( numVertices <= 2 )
        {
            logger_.syslog( "Operational safety polygon specified but fewer than 3 vertices.", Syslog::ERROR );
            return false;
        }

        for( k = 0; k <= ( numVertices - 1 ); k++ )
        {
            xVertex[k] = ( lonVertex_[k] - lonVertex_[0] ) * ( Location::EARTH_RADIUS ) * cos( latVertex_[0] );
            yVertex[k] = ( latVertex_[k] - latVertex_[0] ) * Location::EARTH_RADIUS;
        }

        x = ( longitude - lonVertex_[0] ) * ( Location::EARTH_RADIUS * cos( latVertex_[0] ) );
        y = ( latitude - latVertex_[0] ) * Location::EARTH_RADIUS;

        insidePolygon_ = true;
        for( k = 0; k <= ( numVertices - 1 ); k++ )
        {
            if( k == ( numVertices - 1 ) )
            {
                sideLengthSquared = ( ( xVertex[0] - xVertex[k] ) * ( xVertex[0] - xVertex[k] ) ) + ( ( yVertex[0] - yVertex[k] ) * ( yVertex[0] - yVertex[k] ) );
                Xp[k] = ( ( ( xVertex[0] - xVertex[k] ) * ( xVertex[0] - xVertex[k] ) * x ) + ( ( yVertex[0] - yVertex[k] ) * ( ( xVertex[0] * y ) - ( xVertex[k] * y ) - ( xVertex[0] * yVertex[k] ) + ( xVertex[k] * yVertex[0] ) ) ) ) / sideLengthSquared;
                Yp[k] = ( ( ( yVertex[0] - yVertex[k] ) * ( yVertex[0] - yVertex[k] ) * y ) + ( ( xVertex[0] - xVertex[k] ) * ( ( xVertex[0] * yVertex[k] ) - ( xVertex[k] * yVertex[0] ) + ( x * yVertex[0] ) - ( x * yVertex[k] ) ) ) ) / sideLengthSquared;

                CrossProduct[k] = ( ( x - xVertex[k] ) * ( yVertex[0] - yVertex[k] ) ) - ( ( y - yVertex[k] ) * ( xVertex[0] - xVertex[k] ) );
            }
            else
            {
                sideLengthSquared = ( ( xVertex[k + 1] - xVertex[k] ) * ( xVertex[k + 1] - xVertex[k] ) ) + ( ( yVertex[k + 1] - yVertex[k] ) * ( yVertex[k + 1] - yVertex[k] ) );
                Xp[k] = ( ( ( xVertex[k + 1] - xVertex[k] ) * ( xVertex[k + 1] - xVertex[k] ) * x ) + ( ( yVertex[k + 1] - yVertex[k] ) * ( ( xVertex[k + 1] * y ) - ( xVertex[k] * y ) - ( xVertex[k + 1] * yVertex[k] ) + ( xVertex[k] * yVertex[k + 1] ) ) ) ) / sideLengthSquared;
                Yp[k] = ( ( ( yVertex[k + 1] - yVertex[k] ) * ( yVertex[k + 1] - yVertex[k] ) * y ) + ( ( xVertex[k + 1] - xVertex[k] ) * ( ( xVertex[k + 1] * yVertex[k] ) - ( xVertex[k] * yVertex[k + 1] ) + ( x * yVertex[k + 1] ) - ( x * yVertex[k] ) ) ) ) / sideLengthSquared;

                CrossProduct[k] = ( ( x - xVertex[k] ) * ( yVertex[k + 1] - yVertex[k] ) ) - ( ( y - yVertex[k] ) * ( xVertex[k + 1] - xVertex[k] ) );
            }

            insidePolygon_ = insidePolygon_ && ( CrossProduct[k] > 0.0 );
        }

        for( k = 0; k <= ( numVertices - 1 ); k++ )
        {
            if( k == ( numVertices - 1 ) )
            {
                if( ( ( ( Xp[k] >= xVertex[k] ) && ( Xp[k] <= xVertex[0] ) ) || ( ( Xp[k] >= xVertex[0] ) && ( Xp[k] <= xVertex[k] ) ) ) &&
                        ( ( ( Yp[k] >= yVertex[k] ) && ( Yp[k] <= yVertex[0] ) ) || ( ( Yp[k] >= yVertex[0] ) && ( Yp[k] <= yVertex[k] ) ) ) )
                {
                    D[k] = sqrt( ( Xp[k] - x ) * ( Xp[k] - x ) + ( Yp[k] - y ) * ( Yp[k] - y ) );
                }
                else
                {
                    Dv1 = sqrt( ( xVertex[k] - x ) * ( xVertex[k] - x ) + ( yVertex[k] - y ) * ( yVertex[k] - y ) );
                    Dv2 = sqrt( ( xVertex[0] - x ) * ( xVertex[0] - x ) + ( yVertex[0] - y ) * ( yVertex[0] - y ) );

                    if( Dv1 <= Dv2 )
                        D[k] = Dv1;
                    else
                        D[k] = Dv2;
                }
            }
            else
            {
                if( ( ( ( Xp[k] >= xVertex[k] ) && ( Xp[k] <= xVertex[k + 1] ) ) || ( ( Xp[k] >= xVertex[k + 1] ) && ( Xp[k] <= xVertex[k] ) ) || ( xVertex[k] == xVertex[k + 1] ) ) &&
                        ( ( ( Yp[k] >= yVertex[k] ) && ( Yp[k] <= yVertex[k + 1] ) ) || ( ( Yp[k] >= yVertex[k + 1] ) && ( Yp[k] <= yVertex[k] ) ) || ( yVertex[k] == yVertex[k + 1] ) ) )
                {
                    D[k] = sqrt( ( Xp[k] - x ) * ( Xp[k] - x ) + ( Yp[k] - y ) * ( Yp[k] - y ) );
                }
                else
                {
                    Dv1 = sqrt( ( xVertex[k] - x ) * ( xVertex[k] - x ) + ( yVertex[k] - y ) * ( yVertex[k] - y ) );
                    Dv2 = sqrt( ( xVertex[k + 1] - x ) * ( xVertex[k + 1] - x ) + ( yVertex[k + 1] - y ) * ( yVertex[k + 1] - y ) );

                    if( Dv1 <= Dv2 )
                        D[k] = Dv1;
                    else
                        D[k] = Dv2;
                }
            }
        }

        shortestDistToPolygonSides_ = D[0];
        sideShortestDist_ = 0;

        for( k = 1; k <= ( numVertices - 1 ); k++ )
        {
            if( D[k] < shortestDistToPolygonSides_ )
            {
                shortestDistToPolygonSides_ = D[k];
                sideShortestDist_ = k;
            }
        }

        if( verbosity_ > 0 )
        {
            logger_.syslog( "Vehicle location: " + Str( latitude * 180.0 / 3.14159265 ) + ", " + Str( longitude * 180.0 / 3.14159265 ) + ". Distances to polygon sides: " + Str( D[0] ) + "m," + Str( D[1] ) + "m," + Str( D[2] ) + "m," + Str( D[3] ) + "m. shortestDistToPolygonSides_ =  " + Str( shortestDistToPolygonSides_ ) + " m to side " + Str( sideShortestDist_ ), Syslog::DEBUG );

            logger_.syslog( "CrossProducts of vehicle vector and polygon side vector: " + Str( CrossProduct[0] ) + ", " + Str( CrossProduct[1] ) + ", " + Str( CrossProduct[2] ) + ", " + Str( CrossProduct[3] ) + ". insidePolygon_ = " + Str( insidePolygon_ ), Syslog::DEBUG );
        }

        return true;
    }
    else
        return false;
}

void ShortestDistanceToPolygonSidesCalculator::writeData( void )
{
    dataTimestamp_ = Timestamp::Now();
    shortestDistToPolygonSidesWriter_->write( Units::METER, shortestDistToPolygonSides_, dataTimestamp_ );
    sideShortestDistWriter_->write( Units::COUNT, sideShortestDist_, dataTimestamp_ );
    insidePolygonWriter_->write( Units::BOOL, insidePolygon_, dataTimestamp_ );
}