/***************************************
 Fake node and segment generation.

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2015 Andrew M. Bishop

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU Affero General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Affero General Public License for more details.

 You should have received a copy of the GNU Affero General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ***************************************/


#include "types.h"
#include "nodes.h"
#include "segments.h"

#include "fakes.h"


/*+ The minimum distance along a segment from a node to insert a fake node. (in km). +*/
#define MINSEGMENT 0.005


/* Local variables (re-initialised by DeleteFakeNodes() function) */

/*+ A set of fake segments to allow start/finish in the middle of a segment. +*/
static Segment fake_segments[4*NWAYPOINTS+1];

/*+ A set of pointers to the real segments underlying the fake segments. +*/
static index_t real_segments[4*NWAYPOINTS+1];

/*+ A set of fake node latitudes and longitudes. +*/
static double fake_lon[NWAYPOINTS+1],fake_lat[NWAYPOINTS+1];

/*+ The previous waypoint. +*/
static int prevpoint=0;


/*++++++++++++++++++++++++++++++++++++++
  Create a pair of fake segments corresponding to the given segment split in two
  (and will create an extra two fake segments if adjacent waypoints are on the
  same segment).

  index_t CreateFakes Returns the fake node index (or a real one in special cases).

  Nodes *nodes The set of nodes to use.

  Segments *segments The set of segments to use.

  int point Which of the waypoints this is.

  Segment *segmentp The segment to split.

  index_t node1 The first node at the end of this segment.

  index_t node2 The second node at the end of this segment.

  distance_t dist1 The distance to the first node.

  distance_t dist2 The distance to the second node.
  ++++++++++++++++++++++++++++++++++++++*/

index_t CreateFakes(Nodes *nodes,Segments *segments,int point,Segment *segmentp,index_t node1,index_t node2,distance_t dist1,distance_t dist2)
{
 index_t fakenode;
 double lat1,lon1,lat2,lon2;

 /* Initialise all the connecting segments to fake values */

 fake_segments[4*point-4].node1=NO_NODE;
 fake_segments[4*point-4].node2=NO_NODE;

 fake_segments[4*point-3].node1=NO_NODE;
 fake_segments[4*point-3].node2=NO_NODE;

 fake_segments[4*point-2].node1=NO_NODE;
 fake_segments[4*point-2].node2=NO_NODE;

 fake_segments[4*point-1].node1=NO_NODE;
 fake_segments[4*point-1].node2=NO_NODE;

 /* Check if we are actually close enough to an existing node */

 if(dist1<=km_to_distance(MINSEGMENT) && dist2>km_to_distance(MINSEGMENT))
   {
    prevpoint=point;
    return(node1);
   }

 if(dist2<=km_to_distance(MINSEGMENT) && dist1>km_to_distance(MINSEGMENT))
   {
    prevpoint=point;
    return(node2);
   }

 if(dist1<=km_to_distance(MINSEGMENT) && dist2<=km_to_distance(MINSEGMENT))
   {
    prevpoint=point;

    if(dist1<dist2)
       return(node1);
    else
       return(node2);
   }

 /* Create the fake node */

 fakenode=NODE_FAKE+point;

 GetLatLong(nodes,node1,NULL,&lat1,&lon1);
 GetLatLong(nodes,node2,NULL,&lat2,&lon2);

 if(lat1>3 && lat2<-3)
    lat2+=2*M_PI;
 else if(lat1<-3 && lat2>3)
    lat1+=2*M_PI;

 fake_lat[point]=lat1+(lat2-lat1)*(double)dist1/(double)(dist1+dist2); /* (dist1+dist2) must be > 0 */
 fake_lon[point]=lon1+(lon2-lon1)*(double)dist1/(double)(dist1+dist2); /* (dist1+dist2) must be > 0 */

 if(fake_lat[point]>M_PI) fake_lat[point]-=2*M_PI;

 /*
  *    node1  fakenode                         node2
  *      #----------*----------------------------#     real_segments[4*point-{4,3}]
  *   
  *      #----------*                                  fake_segments[4*point-4]
  *                 *----------------------------#     fake_segments[4*point-3]
  *   
  *   
  *    node1  fakenode[prevpoint]              node2
  *      #----------*------------------%---------#     real_segments[4*prevpoint-{4,3,1}], real_segments[4*point-{4,3,2}]
  *                              fakenode[point]
  *      #----------*                                  fake_segments[4*prevpoint-4]
  *                 *----------------------------#     fake_segments[4*prevpoint-3]
  *                 *------------------%               fake_segments[4*prevpoint-1]
  *      #-----------------------------%               fake_segments[4*point-4]
  *                                    %---------#     fake_segments[4*point-3]
  *                 *------------------%               fake_segments[4*point-2]
  */

 /* Create the first fake segment */

 fake_segments[4*point-4]=*segmentp;

 fake_segments[4*point-4].node2=fakenode;

 fake_segments[4*point-4].distance=DISTANCE(dist1)|DISTFLAG(segmentp->distance);

 real_segments[4*point-4]=IndexSegment(segments,segmentp);

 /* Create the second fake segment */

 fake_segments[4*point-3]=*segmentp;

 fake_segments[4*point-3].node1=fakenode;

 fake_segments[4*point-3].distance=DISTANCE(dist2)|DISTFLAG(segmentp->distance);

 real_segments[4*point-3]=IndexSegment(segments,segmentp);

 /* Create a third fake segment to join adjacent points if both are fake and on the same real segment */

 if(prevpoint>0 && fake_segments[4*prevpoint-4].node1==node1 && fake_segments[4*prevpoint-3].node2==node2)
   {
    if(DISTANCE(dist1)>DISTANCE(fake_segments[4*prevpoint-4].distance)) /* point is further from node1 than prevpoint */
      {
       fake_segments[4*point-2]=fake_segments[4*prevpoint-3];

       fake_segments[4*point-2].node2=fakenode;

       fake_segments[4*point-2].distance=(DISTANCE(dist1)-DISTANCE(fake_segments[4*prevpoint-4].distance))|DISTFLAG(segmentp->distance);
      }
    else
      {
       fake_segments[4*point-2]=fake_segments[4*prevpoint-4];

       fake_segments[4*point-2].node1=fakenode;

       fake_segments[4*point-2].distance=(DISTANCE(fake_segments[4*prevpoint-4].distance)-DISTANCE(dist1))|DISTFLAG(segmentp->distance);
      }

    real_segments[4*point-2]=IndexSegment(segments,segmentp);

    fake_segments[4*prevpoint-1]=fake_segments[4*point-2];

    real_segments[4*prevpoint-1]=real_segments[4*point-2];
   }

 /* Return the fake node */

 prevpoint=point;

 return(fakenode);
}


/*++++++++++++++++++++++++++++++++++++++
  Create a fake segment connecting a node to itself.

  index_t CreateFakeNullSegment Returns the index of a fake segment.

  Segments *segments The list of segments to use.

  index_t node The node that is to be linked.

  index_t segment The segment that is to be emulated.

  int point The waypoint number.
  ++++++++++++++++++++++++++++++++++++++*/

index_t CreateFakeNullSegment(Segments *segments,index_t node,index_t segment,int point)
{
 Segment *segmentp=LookupSegment(segments,segment,1);

 fake_segments[4*point-2].node1=node;
 fake_segments[4*point-2].node2=node;
 fake_segments[4*point-2].way=segmentp->way;
 fake_segments[4*point-2].distance=0;

 return(4*point-2+SEGMENT_FAKE);
}


/*++++++++++++++++++++++++++++++++++++++
  Re-initialise the fake node data storage.
  ++++++++++++++++++++++++++++++++++++++*/

void DeleteFakeNodes(void)
{
 unsigned int i;

 for(i=0;i<sizeof(fake_segments)/sizeof(fake_segments[0]);i++)
   {
    fake_segments[i].node1=NO_NODE;
    fake_segments[i].node2=NO_NODE;
   }

 for(i=0;i<sizeof(real_segments)/sizeof(real_segments[0]);i++)
    real_segments[i]=NO_SEGMENT;

 prevpoint=0;
}


/*++++++++++++++++++++++++++++++++++++++
  Lookup the latitude and longitude of a fake node.

  index_t fakenode The fake node to lookup.

  double *latitude Returns the latitude

  double *longitude Returns the longitude.
  ++++++++++++++++++++++++++++++++++++++*/

void GetFakeLatLong(index_t fakenode, double *latitude,double *longitude)
{
 index_t whichnode=fakenode-NODE_FAKE;

 *latitude =fake_lat[whichnode];
 *longitude=fake_lon[whichnode];
}


/*++++++++++++++++++++++++++++++++++++++
  Finds the first fake segment associated to a fake node.

  Segment *FirstFakeSegment Returns a pointer to the first fake segment.

  index_t fakenode The fake node to lookup.
  ++++++++++++++++++++++++++++++++++++++*/

Segment *FirstFakeSegment(index_t fakenode)
{
 index_t whichnode=fakenode-NODE_FAKE;

 return(&fake_segments[4*whichnode-4]);
}


/*++++++++++++++++++++++++++++++++++++++
  Finds the next fake segment associated to a fake node.

  Segment *NextFakeSegment Returns a pointer to the next fake segment.

  Segment *fakesegmentp The first fake segment.

  index_t fakenode The node to lookup.
  ++++++++++++++++++++++++++++++++++++++*/

Segment *NextFakeSegment(Segment *fakesegmentp,index_t fakenode)
{
 index_t whichnode=fakenode-NODE_FAKE;

 if(fakesegmentp==&fake_segments[4*whichnode-4])
    return(&fake_segments[4*whichnode-3]);

 if(fakesegmentp==&fake_segments[4*whichnode-3] && fake_segments[4*whichnode-2].node1!=NO_NODE)
    return(&fake_segments[4*whichnode-2]);

 if(fakesegmentp==&fake_segments[4*whichnode-3] && fake_segments[4*whichnode-1].node1!=NO_NODE)
    return(&fake_segments[4*whichnode-1]);

 if(fakesegmentp==&fake_segments[4*whichnode-2] && fake_segments[4*whichnode-1].node1!=NO_NODE)
    return(&fake_segments[4*whichnode-1]);

 return(NULL);
}


/*++++++++++++++++++++++++++++++++++++++
  Finds the fake segment between a real node and a fake node.

  Segment *ExtraFakeSegment Returns a segment between the two specified nodes if it exists.

  index_t realnode The real node.

  index_t fakenode The fake node.
  ++++++++++++++++++++++++++++++++++++++*/

Segment *ExtraFakeSegment(index_t realnode,index_t fakenode)
{
 index_t whichnode=fakenode-NODE_FAKE;

 if(fake_segments[4*whichnode-4].node1==realnode || fake_segments[4*whichnode-4].node2==realnode)
    return(&fake_segments[4*whichnode-4]);

 if(fake_segments[4*whichnode-3].node1==realnode || fake_segments[4*whichnode-3].node2==realnode)
    return(&fake_segments[4*whichnode-3]);

 return(NULL);
}


/*++++++++++++++++++++++++++++++++++++++
  Lookup a fake segment given its index.

  Segment *LookupFakeSegment Returns a pointer to the fake segment.

  index_t fakesegment The index of the fake segment.
  ++++++++++++++++++++++++++++++++++++++*/

Segment *LookupFakeSegment(index_t fakesegment)
{
 index_t whichsegment=fakesegment-SEGMENT_FAKE;

 return(&fake_segments[whichsegment]);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the fake index of a fake segment.

  index_t IndexFakeSegment Returns the fake segment.

  Segment *fakesegmentp The fake segment to look for.
  ++++++++++++++++++++++++++++++++++++++*/

index_t IndexFakeSegment(Segment *fakesegmentp)
{
 index_t whichsegment=(index_t)(fakesegmentp-&fake_segments[0]);

 return(whichsegment+SEGMENT_FAKE);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the real segment underlying a fake segment.

  index_t IndexRealSegment Returns the index of the real segment.

  index_t fakesegment The index of the fake segment.
  ++++++++++++++++++++++++++++++++++++++*/

index_t IndexRealSegment(index_t fakesegment)
{
 index_t whichsegment=fakesegment-SEGMENT_FAKE;

 return(real_segments[whichsegment]);
}


/*++++++++++++++++++++++++++++++++++++++
  Determine if a route between two fake segments is valid or a U-turn.

  int IsFakeUTurn Returns true for a U-turn.

  index_t fakesegment1 The first fake segment.

  index_t fakesegment2 The second fake segment.
  ++++++++++++++++++++++++++++++++++++++*/

int IsFakeUTurn(index_t fakesegment1,index_t fakesegment2)
{
 index_t whichsegment1=fakesegment1-SEGMENT_FAKE;
 index_t whichsegment2=fakesegment2-SEGMENT_FAKE;

 if(fake_segments[whichsegment1].node1==fake_segments[whichsegment2].node1)
    return(1);

 if(fake_segments[whichsegment1].node2==fake_segments[whichsegment2].node2)
    return(1);

 return(0);
}
