# -*- coding: utf-8 -*-
"""
Created on Wed Jan 13 14:08:26 2021

@author: Charlie Ferguson
"""

### Packages for ORTools
from __future__ import print_function
from ortools.constraint_solver import routing_enums_pb2
from ortools.constraint_solver import pywrapcp


#General Packages
import pandas as pd
#import os
import numpy as np
from multiprocessing import Pool
from scipy.spatial import distance
from pathlib import Path
from itertools import repeat
from collections import Counter

#%% Define Type of Solver

Solver="local" #or "server"

if Solver=="local":
    FilePath="{Path to local server}"
elif Solver=="server":
    FilePath="{Path to remote server}"
del Solver


#%% Define Variables
Location="Nairobi" #Location Identifier

Cap_Vehicle=50 #The number of filled containers able to be carried by the collection vehicle
NumSetsArray=np.array([[10, 20, 30,50,70,100,250,500,1000,3000, 5000],  #This defines the number of CBS users and the number of iterations for each
                       [50, 50, 50,50,50,50 ,25, 25, 10, 10,10]])

CBSUsersFileSets=["CBSUsers_Num_10","CBSUsers_Num_20","CBSUsers_Num_30","CBSUsers_Num_50","CBSUsers_Num_70", #This defines the names of the folders (following from previous Python Script 1)
                  "CBSUsers_Num_100","CBSUsers_Num_250","CBSUsers_Num_500","CBSUsers_Num_1000",
                  "CBSUsers_Num_3000","CBSUsers_Num_5000"]


#%% Define Functions
def closest_node(node, nodes):
    closest_index = distance.cdist([node], nodes).argmin()
    return closest_index

def closest_node_distance(node, nodes):
    closest_distance = min(min(distance.cdist([node], nodes,"euclidean")))
    return closest_distance


#%% Computation of Strategy A (this is written to run in a HPC, can be converted in for loop for local machine use)


def PrimaryFunction(i): #Determines how many folders to consider inputs from
    NumSets=NumSetsArray[1,i]
    NumUsers=int(CBSUsersFileSets[i].replace("CBSUsers_Num_",""))
    Num_Vehicle=int(np.ceil(NumUsers/Cap_Vehicle)) #Number of vehicles determined by the capacity of each vehicle
    FinalRouteDistances=[]
    FinalRouteLoads=[]
    FinalRouteNodes=[]
    
    my_file = Path(FilePath+"/OutputFiles/"+Location+"_Outputs/StrategyA/"+"RouteOptimisation_"+str(NumUsers)+"Capacity_"+str(Cap_Vehicle)+".csv")
    if my_file.is_file():
            # file exists
        pass
    else:
        for j in range(0,NumSets):
            #Bring in Distance Matrix created in earlier python scripts
            DistanceMatrix=pd.read_csv(FilePath+"/InputFiles/"+Location+"_Inputs/"+CBSUsersFileSets[i]+"/"+CBSUsersFileSets[i]+"_Set_"+str(j)+ "_DistanceMatrix.csv",header=None)
            DistanceMatrix_Info=pd.read_csv(FilePath+"/InputFiles/"+Location+"_Inputs/"+CBSUsersFileSets[i]+"/"+CBSUsersFileSets[i]+"_Set_"+str(j)+ "_DistanceMatrixComponents.csv")
            DM_Households=DistanceMatrix_Info.iloc[1,0]
        
            DistanceMatrix_Tailored=DistanceMatrix.iloc[0:DM_Households+1, 0:DM_Households+1] #Ensures all CBS user 'nearest points' are incorporated into the distance matrix with the exit point
            #Now the script brings in the CBS user point co-ordinates and converts them to tuples
            CBSUserPoints=pd.read_csv(FilePath+"/InputFiles/"+Location+"_Inputs/"+CBSUsersFileSets[i]+"/"+CBSUsersFileSets[i]+"_Set_"+str(j)+ "_CBSPoints.csv")
            CBSUserPoints=CBSUserPoints[["POINT_X","POINT_Y"]]
            CBSUserPoints = [tuple(x) for x in CBSUserPoints.to_numpy()]

            CBSUserRoadPoints=pd.read_csv(FilePath+"/InputFiles/"+Location+"_Inputs/"+CBSUsersFileSets[i]+"/"+CBSUsersFileSets[i]+"_Set_"+str(j)+ "_PointsCSV.csv")
            CBSUserRoadPoints=CBSUserRoadPoints[["POINT_X","POINT_Y"]]
            CBSUserRoadPoints=CBSUserRoadPoints.iloc[1:DistanceMatrix_Info.iloc[1,0]+1,:]
            
            #Identify how many nearest points had more than a single load (and calculate that load)
            ClosestDemands= [0 for i in range(0,DistanceMatrix_Info.iloc[1,0])]
            for x in range(0, len(CBSUserPoints)):
                ClosestDemands[closest_node(CBSUserPoints[x],CBSUserRoadPoints)-1]+=1  
            
            ClosestDemands1=ClosestDemands
            ExtraVehicleControl=[]
            ExtraVehicleControl.append([0,len(ClosestDemands1)])
            
            #This ensures that if the node load exceeds the vehicle capacity that multiple vehicles are dispatched to the same point
            DistanceMatrix_Altered=DistanceMatrix_Tailored
            if max(ClosestDemands1)>Cap_Vehicle:
                    for p in range(0, len(ClosestDemands1)):
                        if ClosestDemands1[p] > Cap_Vehicle:
                            NVisits=int(np.ceil(ClosestDemands1[p]/Cap_Vehicle))
                            StationRow=DistanceMatrix_Tailored.iloc[p,:]
                            StationRows=pd.DataFrame([StationRow,]*(NVisits-1))
                            DistanceMatrix_Altered=DistanceMatrix_Altered.append(StationRows)
                            ClosestDemands1[p] = ClosestDemands1[p] % Cap_Vehicle
                            ClosestDemands1.extend(repeat(Cap_Vehicle, (NVisits-1)))
                            ExtraVehicleControl.append([p,len(ClosestDemands1)])
                            ##
                        ##
                    DistanceMatrix_Altered=DistanceMatrix_Altered.reset_index(drop=True)
                    DistanceMatrix_Altered.columns = range(DistanceMatrix_Altered.shape[1])
                    DistanceMatrix_AlteredT=DistanceMatrix_Altered.transpose()
                    DistanceMatrix_Altered=DistanceMatrix_Altered.combine_first(DistanceMatrix_AlteredT)
                    DistanceMatrix_Altered = DistanceMatrix_Altered.fillna(999999) #This means that these 'pseudo points' are not viable in distance calculations
                    ##
            
            DistanceMatrix_Tailored=DistanceMatrix_Altered.values
            np.fill_diagonal(DistanceMatrix_Tailored,0)
            ClosestDemands1=[0]+ClosestDemands1   
            Vehicle_Capacities=[Cap_Vehicle for i in range(Num_Vehicle)]
        
        
            ######
            #Routing Exercise - THIS IS BASED ON GOOGLE OR-TOOLS CAPACITY-DRIVEN VEHICLE ROUTING PROBLEM (SEE FOR DETAILS/DOCUMENTATION) https://developers.google.com/optimization/routing/cvrp
            def create_data_model():
                """Stores the data for the problem."""
                data = {}
                data['distance_matrix'] = DistanceMatrix_Tailored 
                data['num_vehicles'] = Num_Vehicle
                data['depot'] = 0 #This has been scripted from the 'Exit Point' defined in Script 1
                data['demands'] = ClosestDemands1
                data['vehicle_capacities'] = Vehicle_Capacities
                return data
            
            global OutputRouteDistances
            global OutputRouteLoads
            global NodeRoutes
            NodeRoutes=[]
            OutputRouteDistances=[]
            OutputRouteLoads=[]
            def print_solution(data, manager, routing, solution):
                """Prints solution on console."""
                global OutputRouteDistances
                global OutputRouteLoads
                global NodeRoutes
                NodeRoutes=[]
                OutputRouteDistances=[]
                OutputRouteLoads=[]
                total_distance = 0
                total_load = 0
                for vehicle_id in range(data['num_vehicles']):
                    index = routing.Start(vehicle_id)
                    plan_output = 'Route for vehicle {}:\n'.format(vehicle_id)
                    route_distance = 0
                    route_load = 0
                    previous_index=0
                    while not routing.IsEnd(index):
                        node_index = manager.IndexToNode(index)
                        route_load += data['demands'][node_index]
                        plan_output += ' {0} Load({1}) -> '.format(node_index, route_load)
                        NodeRoutes.append([previous_index,index])
                        previous_index = index
                        index = solution.Value(routing.NextVar(index))
                        route_distance += routing.GetArcCostForVehicle(
                            previous_index, index, vehicle_id)
                    plan_output += ' {0} Load({1})\n'.format(manager.IndexToNode(index),
                                                 route_load)
                    plan_output += 'Distance of the route: {}m\n'.format(route_distance)
                    OutputRouteDistances.append(route_distance)
                    plan_output += 'Load of the route: {}\n'.format(route_load)
                    OutputRouteLoads.append(route_load)
                    total_distance += route_distance
                    total_load += route_load
                return OutputRouteDistances, OutputRouteLoads,NodeRoutes #These outputs (alterations from base Google OR Tools script) are stored for later evaluation



            def main():
                data = create_data_model()
                manager = pywrapcp.RoutingIndexManager(len(data['distance_matrix']),
                                               data['num_vehicles'], data['depot'])
                routing = pywrapcp.RoutingModel(manager)
                def distance_callback(from_index, to_index):
                    """Returns the distance between the two nodes."""
                    # Convert from routing variable Index to distance matrix NodeIndex.
                    from_node = manager.IndexToNode(from_index)
                    to_node = manager.IndexToNode(to_index)
                    return data['distance_matrix'][from_node][to_node]
                transit_callback_index = routing.RegisterTransitCallback(distance_callback)
                routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)

             # Add Capacity constraint.
                def demand_callback(from_index):
                    """Returns the demand of the node."""
                # Convert from routing variable Index to demands NodeIndex.
                    from_node = manager.IndexToNode(from_index)
                    return data['demands'][from_node]

                demand_callback_index = routing.RegisterUnaryTransitCallback(
                    demand_callback)
                routing.AddDimensionWithVehicleCapacity(
                    demand_callback_index,
                    0,  # null capacity slack
                    data['vehicle_capacities'],  # vehicle maximum capacities
                    True,  # start cumul to zero
                    'Capacity')
        
            # Setting first solution heuristic.
                search_parameters = pywrapcp.DefaultRoutingSearchParameters()
                search_parameters.first_solution_strategy = (
                    routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC)
                search_parameters.local_search_metaheuristic = (
                    routing_enums_pb2.LocalSearchMetaheuristic.GUIDED_LOCAL_SEARCH)
                search_parameters.time_limit.FromSeconds(1)
                search_parameters.time_limit.seconds = 300

                # Solve the problem.
                solution = routing.SolveWithParameters(search_parameters)
            
                global routes
                def get_routes(solution, routing, manager):
                    routes = []
                    for route_nbr in range(routing.vehicles()):
                        index = routing.Start(route_nbr)
                        route = [manager.IndexToNode(index)]
                        while not routing.IsEnd(index):
                            index = solution.Value(routing.NextVar(index))
                            route.append(manager.IndexToNode(index))
                        routes.append(route)
                        return routes
            
                routes = get_routes(solution, routing, manager)
                if solution:
                    print_solution(data, manager, routing, solution)

            if __name__ == '__main__':
                main()
            #END OF GOOGLE OR-TOOLS SCRIPT
            
            FinalRouteDistances.append(OutputRouteDistances)
            FinalRouteLoads.append(OutputRouteLoads)
            
            
           #######
           #Post-processing
            
           #These loops ensure the splitting of the different vehicle routes (as they are all appended together in G-OR Tools computation)
            NodeRoutes1 = NodeRoutes
            for x in range(0, len(NodeRoutes1)):
                if NodeRoutes1[x][0]==0:
                    NodeRoutes1[x][1]=0
            for x in range(0, len(NodeRoutes1)):
                if NodeRoutes1[x]==[0,0]:
                    NodeRoutes1[x+1][0]=0
            for x in range(0, len(NodeRoutes1)):
                if NodeRoutes1[x]==[0,0]:
                    NodeRoutes1[x][0]=NodeRoutes1[x-1][1]
            NodeRoutes1=NodeRoutes1[1:]
            NodeRoutes2=pd.DataFrame(NodeRoutes1) 
            
            #This corrects the routes of the extra vehicles (brought above to serve nodes with loads greater than vehicle capacity)
            ExtraVehicleControlReversed=list(reversed(ExtraVehicleControl))
            ExtraVehicleControlReversed=ExtraVehicleControlReversed[:-1]
            if NodeRoutes2.values.max()>len(CBSUserRoadPoints):
                for t in range(0,len(ExtraVehicleControl)):
                    NodeRoutes2[NodeRoutes2>ExtraVehicleControl[t][1]] = ExtraVehicleControl[t][0]
           
            #This accounts for the entrance and exit point
            CBSUserRoadPoints_Corrected=CBSUserRoadPoints
            CBSUserRoadPoints_Corrected.loc[-1] = [0,0]  # Adds the datum
            CBSUserRoadPoints_Corrected.index = CBSUserRoadPoints_Corrected.index + 1  # Correcting index
            CBSUserRoadPoints_Corrected = CBSUserRoadPoints_Corrected.sort_index() 
            
            #Combining of output data
            NodeRoutes_Locations = pd.DataFrame(index=NodeRoutes2.index,columns=NodeRoutes2.columns)
            NodeRoutes_Locations=pd.concat([NodeRoutes_Locations,NodeRoutes_Locations],axis=1)
            NodeRoutes_Locations.columns=['Ax','Ay','Bx','By']
            for x in range(0, len(NodeRoutes_Locations)):
               NodeRoutes_Locations.iloc[x,0]=CBSUserRoadPoints_Corrected .iloc[NodeRoutes2.iloc[x,0],0]
               NodeRoutes_Locations.iloc[x,1]=CBSUserRoadPoints_Corrected .iloc[NodeRoutes2.iloc[x,0],1]
               NodeRoutes_Locations.iloc[x,2]=CBSUserRoadPoints_Corrected .iloc[NodeRoutes2.iloc[x,1],0]
               NodeRoutes_Locations.iloc[x,3]=CBSUserRoadPoints_Corrected .iloc[NodeRoutes2.iloc[x,1],1]   ###
               ##    
            NodeRoutes_Locations_List=NodeRoutes_Locations.values.tolist()
            FinalRouteNodes.extend(NodeRoutes_Locations_List)
            ##
        
        #Tidying of column names
        FinalRouteDistances_df=pd.DataFrame(FinalRouteDistances)
        FinalRouteDistances_df.columns = ['FinalDistanceRoute_'+str(c) for c in FinalRouteDistances_df]
        FinalRouteLoads_df=pd.DataFrame(FinalRouteLoads)
        FinalRouteLoads_df.columns = ['FinalLoadRoute_'+str(c) for c in FinalRouteLoads_df]

        Export_df = pd.concat([FinalRouteDistances_df, FinalRouteLoads_df], axis=1)
        Export_df.to_csv(FilePath+"/OutputFiles/"+Location+"_Outputs/StrategyA/"+"RouteOptimisation_"+str(NumUsers)+"Capacity_"+str(Cap_Vehicle)+".csv")
        ###
    #Save outputs to relevant directory
    FinalRouteNodes_df=pd.DataFrame(FinalRouteNodes)
    FinalRouteNodes_df_Count=FinalRouteNodes_df.groupby(FinalRouteNodes_df.columns.tolist(),as_index=False).size()
    FinalRouteNodes_df_Count.to_csv(FilePath+"/OutputFiles/"+Location+"_Outputs/StrategyA/"+"RouteNodes_"+str(NumUsers)+"Capacity_"+str(Cap_Vehicle)+".csv")
           
if __name__ == '__main__':
    pool = Pool()  #os.cpu_count() - 1
    pool.map(PrimaryFunction, range(0,len(CBSUsersFileSets)))  

