////////////////////////////////////////////////////////////
//class CPolicy implementation
#include "stdafx.h"
#include <math.h>
#include <float.h>
#include "general.h"

#include "SimulationParams.h"
#include "policy.h"

#include "Simulation.h"

int CPolicy::m_CurrConf = 0;

int CPolicy::GetNextConfiguration(CStateAction& nextState, int* WaitTimeArray)
{
	
	
	return ((++m_CurrConf)%8 + 1);
}

/////////////////////////////////////////////////////////////
//class CSarsaPolicy implementation.  
//
//This class generates the actions to be taken using Sarsa learning
//algorithm.  It responds to queries from the simulation manager. It
//returns the epsilon greedy next configuration.
int CSarsaPolicy::GetNextConfiguration(CStateAction& nextState, int* WaitTimeArray)
{
	CStateValue v;
	double maxValue = 0;//assuming R(S,A) < 0
	int maxAction = -1;   //to indicate if an action was found.
	int maxWaitTime1=0;
	int maxWaitTime2=0;
	int WaitLane1 = -1;
	int WaitLane2 = -1;
	static int  index =10;
	//this or loop takes care of bounding the maximal time 
	//of wait in any direction.
	for(int i = 0; i < NUM_OF_LANES;i++)
	{
		if (WaitTimeArray[i] > maxWaitTime1)
		{
			maxWaitTime1 = WaitTimeArray[i];
			WaitLane1 = i; 
			//There might be a possibility of two lanes with 
			//the same wait time
			for (int j=i+1; j < NUM_OF_LANES; j++)
			{
				if (WaitTimeArray[j] == maxWaitTime1)
				WaitLane2 = j;
				maxWaitTime2 = WaitTimeArray[j];
			}
		}
			
	}
	if (maxWaitTime1 >= MAX_WAIT_TIME)
	{
		//If there are two lanes waiting the same amount of time,
		//then neccesarily there exists a configuration in which
		//they can both be active.  We find this configuration. 
		
		
		if (maxWaitTime1 == maxWaitTime2)
		{
			for(int conf = 1; conf < NUM_OF_CONFIGURATIONS;conf++)
			{
				if (CLane::isLaneActive(conf, WaitLane1+1)&&
					CLane::isLaneActive(conf, WaitLane2+1))
					return conf;
			}

		}
		else
		{
			//We find one of the configurations in which the 
			//lane is active
		    for(int conf = 1; conf < NUM_OF_CONFIGURATIONS;conf++)
			{
				if (CLane::isLaneActive(conf, WaitLane1+1))
					return conf;
			}
		
		}
	}
	
	
	
   //finding the argmax of Q(S,A)	
	
	for( i = 1; i < NUM_OF_CONFIGURATIONS;i++)
	{
		nextState.m_nNextConfig = i;
		if(m_StateActionMap.Lookup(nextState,v))
		{
			if(maxValue < v.m_value || (maxValue == 0)) 
			{
				maxValue = v.m_value;
				maxAction = i;
			}
		}
	}
	//picking a random action with probability epsilon
	double epsilon = 1/log(index);
	index++;
	if(epsilon < 0.05) epsilon = 0.05;
 	if (((double)rand()/RAND_MAX < epsilon) || maxAction == -1)	{
		maxAction = (rand() % 8) + 1;
	}
	return maxAction;

}

void CSarsaPolicy::UpdateStateValues(CStateAction &currState,
						CStateAction &nextState, double immedReward)
{
	CStateValue currStateVal, nextStateVal;
	double alpha;

	if(!m_StateActionMap.Lookup(currState, currStateVal))
		currStateVal.m_value = 0;
	if(!m_StateActionMap.Lookup(nextState, nextStateVal))
		nextStateVal.m_value = 0;

	alpha = (double)1/((++(currStateVal.m_nTimesVisited)));
	// Q(S,A) = Q(S,A) + alpha(R(S,A) + lambda(Q(S',A)) - Q(S,A))
	currStateVal.m_value = currStateVal.m_value + alpha *(
		immedReward + LAMBDA * nextStateVal.m_value - currStateVal.m_value);
	ASSERT(currStateVal.m_value > -0.8*DBL_MAX && currStateVal.m_value <= 0);
	m_StateActionMap[currState] = currStateVal;
	
}

double CPolicy::ComputeImReward(CStateAction &nextState, int* WaitTimeArray,
								int* NumOfCarsArr)
{
	double reward = 0;
	int temp;
	for(int i = 0;i < NUM_OF_LANES;i++)
	{
		temp = NumOfCarsArr[i];
		
//		if(temp > MAX_NUM_OF_CARS_IN_JUNC)
//			temp = MAX_NUM_OF_CARS_IN_JUNC;
		reward -= (pow(temp ,m_numOfCarsPow )) * (pow((double)WaitTimeArray[i]/(double)DELTA_T, m_WaitTimePow));
		//reward -= (temp * temp)/1000000;
	}
	return reward;	
}
CProportionalPolicy::CProportionalPolicy(CStateActionMap& SAMap, CSimulationParams& simParams )
	: CPolicy(SAMap),
	m_NorthLambda(0),
	m_EastLambda(0),
	m_SouthLambda(0),
	m_WestLambda(0),
	m_SimParams(simParams)
{
}	
void CProportionalPolicy::SetLanesTraffic()
{
	m_NorthLambda = m_SimParams.m_LaneArrivalArr[0] + m_SimParams.m_LaneArrivalArr[1];
	m_EastLambda  = m_SimParams.m_LaneArrivalArr[2] + m_SimParams.m_LaneArrivalArr[3];
	m_SouthLambda = m_SimParams.m_LaneArrivalArr[4] + m_SimParams.m_LaneArrivalArr[5];
	m_WestLambda  = m_SimParams.m_LaneArrivalArr[6] + m_SimParams.m_LaneArrivalArr[7];
	
	m_SumLambdas = m_NorthLambda+m_EastLambda+m_SouthLambda+m_WestLambda ;

	m_FirstLightChange  = (int)((m_NorthLambda/m_SumLambdas)*JUNC_CYCLE_LENGTH );
	m_SecondLightChange = m_FirstLightChange  + (int)((m_EastLambda/m_SumLambdas)*JUNC_CYCLE_LENGTH) ;
	m_ThirdLightChange  = m_SecondLightChange + (int)((m_SouthLambda/m_SumLambdas)*JUNC_CYCLE_LENGTH) ;
	m_FourthLightChange = m_ThirdLightChange  + (int)((m_WestLambda/m_SumLambdas)*JUNC_CYCLE_LENGTH) ; 
	


}

int CProportionalPolicy::GetNextConfiguration(CStateAction& nextState, int* WaitTimeArray)
{
	
	static int nTimeInCycle = -1;	
	if (nTimeInCycle >= m_FourthLightChange - 1) 
		nTimeInCycle = -1;
	nTimeInCycle++;
	if (nTimeInCycle < m_FirstLightChange)
		return C_NORTH_SOUTH_NORTH_EAST;
	if (nTimeInCycle < m_SecondLightChange)
		return C_EAST_WEST_EAST_SOUTH;
	if (nTimeInCycle < m_ThirdLightChange)
		return C_SOUTH_NORTH_SOUTH_WEST;
	else
		return C_WEST_EAST_WEST_NORTH;
}