/* -*-	Mode:C++; c-basic-offset:8; tab-width:8; indent-tabs-mode:t -*- */
/*
 * Copyright (c) 2006 National ICT Australia Ltd
 * All rights reserved.
 * Author:  E. LOCHIN,
 * 
 * Permission to use and copy this software in source and binary forms
 * is hereby granted, provided that the above copyright notice, this
 * paragraph and the following disclaimer are retained in any copies
 * of any part of this software and that the University of La Reunion is
 * acknowledged in all documentation pertaining to any such copy
 * or derivative work. The name of the University of La Reunion may not
 * be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
 * EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE WARRANTIES OF 
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL 
 * THE UNIVERSITY OF LA REUNION BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER  
 * DEALINGS IN THE SOFTWARE.
 */

/*
 * K O H O N E N
 * 
 * Modified from code from the net. Credit goes to:
 * Karsten Kutza
 * http://www.neural-networks-at-your-fingertips.com/
 */

#include <math.h>
#include <sys/types.h>
#include "config.h"
#include "template.h"
#include "random.h"
#include "flags.h"
#include "delay.h"
#include "kred.h"

static class KREDClass:public TclClass {
  public:
    KREDClass():TclClass("Queue/RED/KRED") {
    } TclObject *create(int argc, const char *const *argv) {
	return (new KREDQueue());
    }
}

class_kred;

KREDQueue::KREDQueue():REDQueue()
{
    n = 0;
    first = 0;
    rwdone = 0;
    InitializeRandoms();
    GenerateNetwork(&Net);
    RandomWeights(&Net);
    InitializeApplication(&Net);
    bind("TrainSteps_", &TrainSteps_);
    bind("tdebug_", &tdebug_);
    bind("curqlen_method_", &curqlen_method_);
    steps = Init1;
    openfile();
    start_time = Scheduler::instance().clock();
}

void KREDQueue::openfile()
{
    if (1) {
	if ((fp = fopen("kred-traces.tr", "w")) == NULL) {
	    fprintf(stderr, "Problem with opening the trace file\n");
	    abort();
	} else
	    printf("---Trace file open---\n");
    } else
	printf("No trace file\n");
}

double KREDQueue::estimator(int nqueued, int m, double ave, double q_w)
{
    double new_ave, old_ave;
    double kqlen_new, kqlen_old;
    
    kqlen_old = kqlen_new;
    kqlen_new = qib_ ? q_->byteLength() : q_->length();

    new_ave = ave;
    while (--m >= 1) {
	new_ave *= 1.0 - q_w;
    }
    old_ave = new_ave;
    new_ave *= 1.0 - q_w;
    new_ave += q_w * nqueued;

    double now = Scheduler::instance().clock();
    if (edp_.adaptive == 1) {
	if (edp_.feng_adaptive == 1)
	    updateMaxPFeng(new_ave);
	else if (now > edv_.lastset + edp_.interval)
	    updateMaxP(new_ave, now);
    } else {
    	fprintf(fp, "Current_Q: %f\n", (double) kqlen_new); 
	if (now > edv_.lastset + edp_.interval) {
		if (curqlen_method_ == 1)
	    		updateMaxKohonen(&Net, kqlen_new, kqlen_old, now);
		else
	    		updateMaxKohonen(&Net, new_ave, old_ave, now);
	}
    }

    if (curqlen_method_ == 1)
	    return kqlen_new;
    else
	    return new_ave;
}

#define	DTYPE_NONE	0	/* ok, no drop */
#define	DTYPE_FORCED	1	/* a "forced" drop */
#define	DTYPE_UNFORCED	2	/* an "unforced" (random) drop */

void KREDQueue::enque(Packet * pkt)
{
    /*
     * if we were idle, we pretend that m packets arrived during
     * the idle period.  m is set to be the ptc times the amount
     * of time we've been idle for
     */

    int m = 0;
    if (idle_) {
	// A packet that arrives to an idle queue will never
	//  be dropped.
	double now = Scheduler::instance().clock();
	/* To account for the period when the queue was empty. */
	idle_ = 0;
	// Use idle_pktsize instead of mean_pktsize, for
	//  a faster response to idle times.
	if (edp_.cautious == 3) {
	    double ptc = edp_.ptc * edp_.mean_pktsize / edp_.idle_pktsize;
	    m = int (ptc * (now - idletime_));
	} else
	    m = int (edp_.ptc * (now - idletime_));
    }

    /*
     * Run the estimator with either 1 new packet arrival, or with
     * the scaled version above [scaled by m due to idle time]
     */
    edv_.v_ave =
	estimator(qib_ ? q_->byteLength() : q_->length(), m + 1,
		  edv_.v_ave, edp_.q_w);
    //printf("v_ave: %6.4f (%13.12f) q: %d)\n", 
    //      double(edv_.v_ave), double(edv_.v_ave), q_->length());
    if (summarystats_) {
	/* compute true average queue size for summary stats */
	Queue::updateStats(qib_ ? q_->byteLength() : q_->length());
    }

    /*
     * count and count_bytes keeps a tally of arriving traffic
     * that has not been dropped (i.e. how long, in terms of traffic,
     * it has been since the last early drop)
     */

    hdr_cmn *ch = hdr_cmn::access(pkt);
    ++edv_.count;
    edv_.count_bytes += ch->size();

    /*
     * DROP LOGIC:
     *      q = current q size, ~q = averaged q size
     *      1> if ~q > maxthresh, this is a FORCED drop
     *      2> if minthresh < ~q < maxthresh, this may be an UNFORCED drop
     *      3> if (q+1) > hard q limit, this is a FORCED drop
     */

    register double qavg = edv_.v_ave;
    int droptype = DTYPE_NONE;
    int qlen = qib_ ? q_->byteLength() : q_->length();
    int qlim = qib_ ? (qlim_ * edp_.mean_pktsize) : qlim_;

    curq_ = qlen;		// helps to trace queue during arrival, if enabled

    if (qavg >= edp_.th_min && qlen > 1) {
	if ((!edp_.gentle && qavg >= edp_.th_max) ||
	    (edp_.gentle && qavg >= 2 * edp_.th_max)) {
	    droptype = DTYPE_FORCED;
	} else if (edv_.old == 0) {
	    /* 
	     * The average queue size has just crossed the
	     * threshold from below to above "minthresh", or
	     * from above "minthresh" with an empty queue to
	     * above "minthresh" with a nonempty queue.
	     */
	    edv_.count = 1;
	    edv_.count_bytes = ch->size();
	    edv_.old = 1;
	} else if (drop_early(pkt)) {
	    droptype = DTYPE_UNFORCED;
	}
    } else {
	/* No packets are being dropped.  */
	edv_.v_prob = 0.0;
	edv_.old = 0;
    }
    if (qlen >= qlim) {
	// see if we've exceeded the queue size
	droptype = DTYPE_FORCED;
    }

    if (droptype == DTYPE_UNFORCED) {
	/* pick packet for ECN, which is dropping in this case */
	Packet *pkt_to_drop = pickPacketForECN(pkt);
	/* 
	 * If the packet picked is different that the one that just arrived,
	 * add it to the queue and remove the chosen packet.
	 */
	if (pkt_to_drop != pkt) {
	    q_->enque(pkt);
	    q_->remove(pkt_to_drop);
	    pkt = pkt_to_drop;	/* XXX okay because pkt is not needed anymore */
	}
	// deliver to special "edrop" target, if defined
	if (de_drop_ != NULL) {

	    //trace first if asked 
	    // if no snoop object (de_drop_) is defined, 
	    // this packet will not be traced as a special case.
	    if (EDTrace != NULL)
		((Trace *) EDTrace)->recvOnly(pkt);

	    reportDrop(pkt);
	    de_drop_->recv(pkt);
	} else {
	    reportDrop(pkt);
	    drop(pkt);
	}
    } else {
	/* forced drop, or not a drop: first enqueue pkt */
	q_->enque(pkt);

	/* drop a packet if we were told to */
	if (droptype == DTYPE_FORCED) {
	    /* drop random victim or last one */
	    pkt = pickPacketToDrop();
	    q_->remove(pkt);
	    reportDrop(pkt);
	    drop(pkt);
	    if (!ns1_compat_) {
		// bug-fix from Philip Liu, <phill@ece.ubc.ca>
		edv_.count = 0;
		edv_.count_bytes = 0;
	    }
	}
    }
    return;
}

void KREDQueue::updateMaxKohonen(NET * Net, double new_ave, double old_ave,
				 double now)
{
    if (n < TrainSteps_) {

	if (steps != Step1) {
	    if (edp_.th_min < new_ave && new_ave < edp_.th_max) {
		edv_.status = edv_.Between;
		n++;
		Net->Alpha = 0.5 * pow(0.01, (double) n / TrainSteps_);
		Net->Alpha_ = 0.5 * pow(0.01, (double) n / TrainSteps_);
		Net->Alpha__ = 0.005;
		Net->Gamma = 0.05;
		Net->Sigma = 6.0 * pow(0.2, (double) n / TrainSteps_);
		Input[0] = old_ave;
		Input[1] = new_ave;
		SetInput(Net, Input);
		PropagateNet(Net);
		GetOutput(Net, Output);
		edv_.cur_max_p = Output[0];
		StepSize = Net->KohonenLayer->StepSize[Net->Winner];
		double raand = RandomNormaldouble(0, 1);
		edv_.cur_max_p += StepSize * raand;
		if (edv_.cur_max_p <= 0)
		    edv_.cur_max_p = 0.01;
		if (edv_.cur_max_p > 1)
		    edv_.cur_max_p = 1;

		if (tdebug_)
		    fprintf(fp,
			    "KRED_STEP1 max_p: %f stepsize %f random %f trainsteps %d\n",
			    (double) edv_.cur_max_p, (double) StepSize,
			    raand, n); 
		steps = Step1;
	    } else {
		/* init process */
		InitKRED(new_ave);
		steps = Init1;

		if (tdebug_)
		    fprintf(fp, "KRED_INIT1 max_p: %f\n",
			    (double) edv_.cur_max_p); 
	    }

	} else {
	    if (edp_.th_min < new_ave && new_ave < edp_.th_max) {
		edv_.status = edv_.Between;
		ScoreOld = -(old_ave * old_ave);
		ScoreNew = -(new_ave * new_ave);
		dScore = ScoreNew - ScoreOld;
		dScoreMean = Net->KohonenLayer->dScoreMean[Net->Winner];
		if (dScore > dScoreMean) {
		    Target[0] = edv_.cur_max_p;
		    TrainUnits(Net, Input, Target);
		}
		Net->KohonenLayer->dScoreMean[Net->Winner] +=
		    Net->Gamma * (dScore - dScoreMean);

		if (tdebug_)
		    fprintf(fp, "KRED_STEP2 max_p: %f\n",
			    (double) edv_.cur_max_p);
		steps = Step2;
	    } else {
		/* init process */
		InitKRED(new_ave);
		steps = Init2;
		if (tdebug_)
		    fprintf(fp, "KRED_INIT2 max_p: %f\n",
			    (double) edv_.cur_max_p); 
	    }
	}
    } else {
	    if (TrainSteps_ != -1) {
		    if (rwdone != 1) {
			    WriteNet(Net);
			    rwdone = 1;
		    }
	    } else {
		    if (rwdone != 1) {
			    ReadNet(Net);
			    rwdone = 1;
		    }
	    }
		    
	    if (edp_.th_min < new_ave && new_ave < edp_.th_max) {
	    	if (first == 0 && TrainSteps_ != -1) {
			if (tdebug_)
				fprintf(fp, "TRAIN STEPS FINISHED at %f\n", Scheduler::instance().clock() - start_time );
			printf("TRAIN STEPS FINISHED at %f\n", Scheduler::instance().clock() - start_time );
			first = 1;
			//edv_.cur_max_p = 1.0 / edp_.max_p_inv;
	    	}
	    	n++;
	    	Input[0] = old_ave;
	    	Input[1] = new_ave;
	    	SetInput(Net, Input);
	    	PropagateNet(Net);
	    	GetOutput(Net, Output);
	    	edv_.cur_max_p = Output[0];
	    	if (debug_)
			fprintf(fp, "KRED_OK max_p: %g %f %f\n", Scheduler::instance().clock(),
				(double) edv_.cur_max_p, (double) new_ave);
		} else {
	    	if (debug_)
			fprintf(fp, "KRED_NOT_OK max_p: %g %f %f\n", Scheduler::instance().clock(),
				(double) edv_.cur_max_p, (double) new_ave);

		}
    	}

    edv_.lastset = now;

}

void KREDQueue::InitKRED(double new_ave)
{
    //edv_.cur_max_p = 1.0 / edp_.max_p_inv;

    if (new_ave < edp_.th_min && edv_.status != edv_.Below) {
	edv_.status = edv_.Below;
	edv_.cur_max_p = edv_.cur_max_p / 3;
    }
    if (new_ave > edp_.th_max && edv_.status != edv_.Above) {
	edv_.status = edv_.Above;
	edv_.cur_max_p = edv_.cur_max_p * 2;
    }
    
    if (edv_.cur_max_p <= 0)
	    edv_.cur_max_p = 0.01;
    if (edv_.cur_max_p > 1)
	    edv_.cur_max_p = 1;
}

/*
 *  pdating max_p, following code from Feng et al. 
 *  This is only called for Adaptive RED.
 *  From "A Self-Configuring RED Gateway", from Feng et al.
 *  They recommend alpha = 3, and beta = 2.
 */
void KREDQueue::updateMaxPFeng(double new_ave)
{
    n++;

    if (edp_.th_min < new_ave && new_ave < edp_.th_max) {
	edv_.status = edv_.Between;
    }
    if (new_ave < edp_.th_min && edv_.status != edv_.Below) {
	edv_.status = edv_.Below;
	edv_.cur_max_p = edv_.cur_max_p / 3;
    }
    if (new_ave > edp_.th_max && edv_.status != edv_.Above) {
	edv_.status = edv_.Above;
	edv_.cur_max_p = edv_.cur_max_p * 2;
    }

    if (debug_)
	fprintf(fp, "FRED max_p: %g %f %f\n", Scheduler::instance().clock(), (double) edv_.cur_max_p, (double) new_ave);
}

/*
 *  Updating max_p to keep the average queue size within the target range.
 *  This is only called for Adaptive RED.
 */
void KREDQueue::updateMaxP(double new_ave, double now)
{
    double part = 0.4 * (edp_.th_max - edp_.th_min);
    // AIMD rule to keep target Q~1/2(th_min+th_max)

    n++;

    if (new_ave < edp_.th_min + part && edv_.cur_max_p > edp_.bottom) {
	// we increase the average queue size, so decrease max_p
	edv_.cur_max_p = edv_.cur_max_p * edp_.beta;
	edv_.lastset = now;
    } else if (new_ave > edp_.th_max - part && edp_.top > edv_.cur_max_p) {
	// we decrease the average queue size, so increase max_p
	double alpha = edp_.alpha;
	if (alpha > 0.25 * edv_.cur_max_p)
	    alpha = 0.25 * edv_.cur_max_p;
	edv_.cur_max_p = edv_.cur_max_p + alpha;
	edv_.lastset = now;
    }

    if (debug_)
	fprintf(fp, "ARED max_p: %g %f %f\n", Scheduler::instance().clock(), (double) edv_.cur_max_p, (double) new_ave);
}

/******************************************************************************
        K O H O N E N
 ******************************************************************************/


void KREDQueue::InitializeRandoms()
{
    srand(4715);
}


double KREDQueue::RandomEqualdouble(double Low, double High)
{
    return ((double) rand() / RAND_MAX) * (High - Low) + Low;
}


double KREDQueue::RandomNormaldouble(double Mu, double Sigma)
{
    double x, fx;

    do {
	x = RandomEqualdouble(Mu - 3 * Sigma, Mu + 3 * Sigma);
	fx = (1 / (sqrt(2 * PI) * Sigma)) * exp(-sqr(x - Mu) /
						(2 * sqr(Sigma)));
    }
    while (fx < RandomEqualdouble(0, 1));
    return x;
}

void KREDQueue::InitializeApplication(NET * Net)
{
    int i;

    for (i = 0; i < Net->KohonenLayer->Units; i++) {
	Net->KohonenLayer->StepSize[i] = 1;
	Net->KohonenLayer->dScoreMean[i] = 0;
    }
}

void KREDQueue::GenerateNetwork(NET * Net)
{
    int i;

    Net->InputLayer = (LAYER *) malloc(sizeof(LAYER));
    Net->KohonenLayer = (LAYER *) malloc(sizeof(LAYER));
    Net->OutputLayer = (LAYER *) malloc(sizeof(LAYER));

    Net->InputLayer->Units = N;
    Net->InputLayer->Output = (double *) calloc(N, sizeof(double));

    Net->KohonenLayer->Units = C;
    Net->KohonenLayer->Output = (double *) calloc(C, sizeof(double));
    Net->KohonenLayer->Weight = (double **) calloc(C, sizeof(double *));
    Net->KohonenLayer->StepSize = (double *) calloc(C, sizeof(double));
    Net->KohonenLayer->dScoreMean = (double *) calloc(C, sizeof(double));

    Net->OutputLayer->Units = M;
    Net->OutputLayer->Output = (double *) calloc(M, sizeof(double));
    Net->OutputLayer->Weight = (double **) calloc(M, sizeof(double *));

    for (i = 0; i < C; i++) {
	Net->KohonenLayer->Weight[i] = (double *) calloc(N, sizeof(double));
    }
    for (i = 0; i < M; i++) {
	Net->OutputLayer->Weight[i] = (double *) calloc(C, sizeof(double));
    }
}


void KREDQueue::RandomWeights(NET * Net)
{
    int i, j;

    for (i = 0; i < Net->KohonenLayer->Units; i++) {
	for (j = 0; j < Net->InputLayer->Units; j++) {
	    Net->KohonenLayer->Weight[i][j] = RandomEqualdouble(-30, 30);
	}
    }
    for (i = 0; i < Net->OutputLayer->Units; i++) {
	for (j = 0; j < Net->KohonenLayer->Units; j++) {
	    Net->OutputLayer->Weight[i][j] = 0;
	}
    }
}


void KREDQueue::SetInput(NET * Net, double * Input)
{
    int i;

    for (i = 0; i < Net->InputLayer->Units; i++) {
	Net->InputLayer->Output[i] = Input[i];
    }

}


void KREDQueue::GetOutput(NET * Net, double * Output)
{
    int i;

    for (i = 0; i < Net->OutputLayer->Units; i++) {
	Output[i] = Net->OutputLayer->Output[i];
    }
}

/******************************************************************************
                     P R O P A G A T I N G   S I G N A L S
 ******************************************************************************/


void KREDQueue::PropagateToKohonen(NET * Net)
{
    int i, j;
    double Out, Weight, Sum, MinOut;

    for (i = 0; i < Net->KohonenLayer->Units; i++) {
	Sum = 0;
	for (j = 0; j < Net->InputLayer->Units; j++) {
	    Out = Net->InputLayer->Output[j];
	    Weight = Net->KohonenLayer->Weight[i][j];
	    Sum += sqr(Out - Weight);
	}
	Net->KohonenLayer->Output[i] = sqrt(Sum);
    }
    MinOut = MAX_double;
    for (i = 0; i < Net->KohonenLayer->Units; i++) {
	if (Net->KohonenLayer->Output[i] < MinOut)
	    MinOut = Net->KohonenLayer->Output[Net->Winner = i];
    }
}


void KREDQueue::PropagateToOutput(NET * Net)
{
    int i;

    for (i = 0; i < Net->OutputLayer->Units; i++) {
	Net->OutputLayer->Output[i] =
	    Net->OutputLayer->Weight[i][Net->Winner];
    }
}


void KREDQueue::PropagateNet(NET * Net)
{
    PropagateToKohonen(Net);
    PropagateToOutput(Net);
}


/******************************************************************************
                        T R A I N I N G   T H E   N E T
 ******************************************************************************/


double KREDQueue::Neighborhood(NET * Net, int i)
{
    int iRow, iCol, jRow, jCol;
    double Distance;

    iRow = i / COLS;
    jRow = Net->Winner / COLS;
    iCol = i % COLS;
    jCol = Net->Winner % COLS;

    Distance = sqrt(sqr(iRow - jRow) + sqr(iCol - jCol));

    return exp(-sqr(Distance) / (2 * sqr(Net->Sigma)));
}


void KREDQueue::TrainKohonen(NET * Net, double Input[2])
{
    int i, j;
    double Out, Weight, Lambda, StepSize;

    for (i = 0; i < Net->KohonenLayer->Units; i++) {
	for (j = 0; j < Net->InputLayer->Units; j++) {
	    Out = Input[j];
	    Weight = Net->KohonenLayer->Weight[i][j];
	    Lambda = Neighborhood(Net, i);
	    Net->KohonenLayer->Weight[i][j] +=
		Net->Alpha * Lambda * (Out - Weight);
	}
	StepSize = Net->KohonenLayer->StepSize[i];
	Net->KohonenLayer->StepSize[i] +=
	    Net->Alpha__ * Lambda * -StepSize;
    }
}


void KREDQueue::TrainOutput(NET * Net, double * Output)
{
    int i, j;
    double Out, Weight, Lambda;

    for (i = 0; i < Net->OutputLayer->Units; i++) {
	for (j = 0; j < Net->KohonenLayer->Units; j++) {
	    Out = Output[i];
	    Weight = Net->OutputLayer->Weight[i][j];
	    Lambda = Neighborhood(Net, j);
	    Net->OutputLayer->Weight[i][j] +=
		Net->Alpha_ * Lambda * (Out - Weight);
	}
    }
}


void KREDQueue::TrainUnits(NET * Net, double * Input, double * Output)
{
    TrainKohonen(Net, Input);
    TrainOutput(Net, Output);
}

void KREDQueue::WriteNet(NET * Net)
{
  int  r,c;
  double x,y,z;
  FILE *fpw;

  if ((fpw = fopen("net-dump.txt", "w")) == NULL) {
    fprintf(stderr, "Problem with opening the writenet file\n");
    abort();
  } else
    printf("---Write Net ---\n");
  
  for (r=0; r<ROWS; r++) {
    for (c=0; c<COLS; c++) {
      x = Net->KohonenLayer->Weight[r*ROWS+c][0];
      y = Net->KohonenLayer->Weight[r*ROWS+c][1];
      z = Net->OutputLayer->Weight[0][r*ROWS+c];
      fprintf(fpw, "%lf %lf %lf ", x, y, z);
    }
    //fprintf(fpw, "\n");
  }
  fclose(fpw);
}

void KREDQueue::ReadNet(NET * Net)
{
  int  r,c;
  double x,y,z;
  FILE *fpr;

  if ((fpr = fopen("net-dump.txt", "r")) == NULL) {
    fprintf(stderr, "Problem with opening the writenet file\n");
    abort();
  } else
    printf("---Read Net ---\n");
  
  for (r=0; r<ROWS; r++) {
    for (c=0; c<COLS; c++) {
      fscanf(fpr, "%lf %lf %lf ", &x, &y, &z);
      Net->KohonenLayer->Weight[r*ROWS+c][0] = x;
      Net->KohonenLayer->Weight[r*ROWS+c][1] = y;
      Net->OutputLayer->Weight[0][r*ROWS+c] = z;
    }
  }
  fclose(fpr);
}