/****************************************************************************
 *  F i l e   D a t a                                                        
 *  $Id: WS_WorkingSchedule_INFN.c,v 1.26 2005/03/20 18:27:47 sebastiani Exp $
 *                                                                           
 ****************************************************************************
 *  S W   D e v e l o p m e n t   E n v i r o n m e n t                      
 *                                                                           
 *  $Author: sebastiani $                                                                   
 *               :                                                           
 ****************************************************************************
 *                                                                           
 *****************************************************************************/


/*============================= Include File ================================*/

#include <src/INFN/LU_SourceFileID_INFN.h>
#define __FILEID__ _WS_WorkingSchedule_INFN__c
#include <src/INFN/LU_LogUtility_INFN.h>
#include <src/INFN/PRH_ParamHandler_INFN_auto.h>
LU_DECL_MASK();

#include <src/INFN/PM_PamManager_INFN.h>
#include <src/INFN/WS_WorkingSchedule_INFN.h>
#include <src/BasicSW/RTEMSInterface/OS_rtems_p.h>
#include <src/INFN/SMH_SelectModeHandler_INFN.h>
#include <src/INFN/CM_Common_INFN.h>
#include <src/INFN/PRH_ParamHandler_INFN_autopri.h>
#include <src/INFN/PRH_ParamHandler_INFN.h>

/*============================ Global define ================================*/


/*============================== global types  ==============================*/

static WS_2_TABLE WS_2_Table[WS_2_N_TABLES];

/* returns the firs and last time value of a table */
#define WS_2_TABLE_FIRST(table) (WS_2_Table[table].basetime)
#define WS_2_TABLE_LAST(table)  WS_2_GetAbsValueEnd(table)
#define WS_2_TABLE_EMPTY(table) (WS_2_Table[table].size == 0)

/*****************************************************************************/
/*=========================== Structure define ==============================*/


/*****************************************************************************/
/*============================ Enumerate define =============================*/

/*****************************************************************************/

static WS WS_FavouriteWorkingSchedule;
static WS WS_EffectiveWorkingSchedule;

static RM_ACQ_SETTING WS_3_NextAcqMode = RM_ACQ_SETTING_B;

/*=== WS    O P E R A T I O N   E N V I R O N M  ==*/

/*****************************************************************************/

void WS_ZeroTableWS2() {
  unsigned short table;
  for(table=0;table<WS_2_N_TABLES;table++)
      WS_2_Table[table].size = 0;  
}

status_code WS_Init() {

  WS_FavouriteWorkingSchedule=PRH_VAR_WS_FAVOURITE_WS;
  WS_EffectiveWorkingSchedule=WS_0;

  return SUCCESSFUL;
}

WS WS_Favourite() {
  return WS_FavouriteWorkingSchedule;
}

WS WS_Effective() {
  return WS_EffectiveWorkingSchedule;
}


/* return the absolute time of the die time of the table */
static TI_TIME WS_2_GetAbsValueEnd(UINT16 table) {
  TI_TIME ret_s=WS_2_Table[table].basetime;
  UINT16 i;
  for(i=0;i<WS_2_Table[table].size;i++) {
    ret_s+=WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S;
  }
  return ret_s;
}


/* delete tables not needed any more */
status_code WS_2_ClearOldTables(TI_TIME now_s) {
  status_code status;
  BYTE table;
  for(table=0;table<WS_2_N_TABLES;table++) {
    if(!WS_2_TABLE_EMPTY(table))
      if(now_s >= WS_2_TABLE_LAST(table)) {
	/* the table is not needed any more , clear it */
	WS_2_Table[table].size = 0;
      }
  }
  return  CM_RC_SUCCESSFUL;
}


static status_code WS_2_GetNextUsefulTableGap(TI_TIME now_s,TI_TIME *gap_s) {
  DVOLATILE int i,j;
#define WS_2_MAX_TIME 0xFFFFFFFF
  DVOLATILE TI_TIME min_basetime=WS_2_MAX_TIME;
  WS_2_ClearOldTables(now_s);
  for(i=0;i<=WS_2_N_TABLES;i++)
    {
      if(WS_2_Table[i].size!=0 && WS_2_Table[i].basetime<min_basetime)
	  min_basetime=WS_2_Table[i].basetime;
    }
  if (min_basetime==WS_2_MAX_TIME)
    return CM_RC_NOTFOUND;
  else {
    *gap_s=(min_basetime-now_s);
    return CM_RC_SUCCESSFUL;
  }
}



/* look for a valid table of a given time. returns the table number in table*/
status_code WS_2_LookForValidTable(TI_TIME now_s,UINT16 *table) {
  status_code status;
  BYTE table_count;
  for(table_count=0;table_count<WS_2_N_TABLES;table_count++) {
    if( !WS_2_TABLE_EMPTY(table_count) &&
	WS_2_TABLE_FIRST(table_count) <= now_s && 
	now_s <= WS_2_TABLE_LAST(table_count)
       ) {
      /* this is a valid table */
      *table = table_count;
      return CM_RC_SUCCESSFUL;
    }
  }
  /* no valid table found */
  return CM_RC_NOTFOUND;
}

/* look for a valid record in the given table. returns the acq infos of the record that match 
   the time. Matching time means to be in an appropriate intervalal 'i' such that
        absolute_starting_time[i] <= now_s < absolute_end_time[i]
 */
status_code WS_2_LookForValidInfo(TI_TIME now_s,UINT16 table,UINT16 *idx,RM_ACQ_SETTING *mode,TI_TIME *left_duration_run_ms) {
  unsigned short i;
  TI_TIME before_s=WS_2_Table[table].basetime;
  TI_TIME after_s;
  for(i=0;i<WS_2_Table[table].size;i++) {
    after_s=before_s+WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S;
    if(before_s <= now_s && now_s < after_s) {
      *left_duration_run_ms = (after_s - now_s)*1000;
      *idx=i;
      *mode                 = (RM_ACQ_SETTING)WS_2_Table[table].record[i].acq_setting;
      return CM_RC_SUCCESSFUL;
    }
    before_s=after_s;
  }
  return CM_RC_NOTFOUND;
}

status_code WS_2_WriteWS2Tables() {
  return PRH_WriteWS2Tables(WS_2_Table);
}

status_code WS_2_ReadWS2Tables() {
  return PRH_ReadWS2Tables(WS_2_Table);
}


status_code WS_2_SetTable(MA_HEADER_MCMD *headerMcmd) {
  /*
    MCMD format explanation ---> into WS_2 Table

    Mi is 1 bit info
    ti is 8 bit info
    k  is coded into 10 bit
    T  is coded into 32 bit
     The table into mcmd look like this:

     -------------
     T
     k
     -------------
     M0  t0
     M1  t1
     ... ...
     Mk-1  tk-1
     -------------

     this info is unpacked into the Table[k] as followind mean:


     Table elem idx      Duration field        Mode
       0                     t0            M0
       1                     t1            M1
       ....                 ....          .....
       k-1                  tk-1          Mk-1

   */
  DVOLATILE unsigned short  *BodyArea, *curPtr;
  DVOLATILE unsigned char offset = 0;
  DVOLATILE unsigned short table,len,i,info,old_info,mcmdlen;
  TI_TIME base_time;
  BodyArea = curPtr = (unsigned short *) SMH_GETPTR_BODYAREA(headerMcmd->PtrMCMD);
  /* skip the 6th word */
  curPtr++;
  mcmdlen=(*curPtr & 0x01FF);
  curPtr++;

  /* skip the STAMP, already checked: */
  curPtr+=2;

  /* get the time info (32 bit): */
  base_time = (*curPtr) << 16;
  curPtr++;
  base_time |=  *curPtr;
  curPtr++;

  /* read the  4 bit, containing the number of the table */
  CM_READ_NEXT_BITS_UINT16(curPtr,offset,4,table);

  if(table >= WS_2_N_TABLES) {
    /*@LOG WS2: invalid table no*/
    LU_INFN_LOG(LU_CRITICAL|LU_HA,LU_MASK(__FILEID__),__FILEID__,__LINE__,table);
    return CM_RC_MCMD_FORMAT_ERROR;
  }
    
  /* read the 10 bit, containing the number of infos */
  CM_READ_NEXT_BITS_UINT16(curPtr,offset,10,len);
  WS_2_Table[table].size = len;
  WS_2_Table[table].basetime=base_time;
  if(len>0) {
    for(i=0;i<len && i<WS_2_TABLE_MAX_ELEM;i++) {
      CM_READ_NEXT_BITS_UINT16(curPtr,offset,9,info);
      WS_2_Table[table].record[i].acq_setting = (info >> 8)&0x1 ? RM_ACQ_SETTING_B : RM_ACQ_SETTING_A;
      WS_2_Table[table].record[i].duration    = ((info & 0x00FF));
    }
  }
  
  return CM_RC_SUCCESSFUL;
}

#ifdef DEBUG

status_code WS_2_LogTable(int table,BOOL full) {
  int i;
  /*@LOG Table no.*/
  LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,table);
  /*@LOG Table basetime*/
  LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].basetime);
  /*@LOG Table size*/
  LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].size);
  if(full)
    for(i=0;i<WS_2_Table[table].size && i<WS_2_TABLE_MAX_ELEM;i++) {
      /*@LOG Table: 0xC0000000 | WS_2_Table[table].record[i].acq_setting | i<<16 */
      LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,0xC0000000 | WS_2_Table[table].record[i].acq_setting | i<<16);
      /*@LOG Table: WS_2_Table[table].record[i].duration */
      LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].record[i].duration);
      /*@LOG Table: WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S */
      LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S);
    }
  
}

status_code WS_2_Log() {
  DVOLATILE int i;
  for(i=0;i<WS_2_N_TABLES;i++) {
    WS_2_LogTable(i,WS_2_FULL);
  }
  
}
#endif


status_code WS_1_GetAcqSetting_and_time(TI_TIME OrbitOffset_ms,
					RM_ACQ_SETTING *acq_mode,
					TI_TIME *duration_run_ms,
					WS *effective_ws) {

  /*

  this is the orbit:

  +-------------------------------------------------------+
  |   A  |      B      |      A      |      B      |  A   |
  |------|------|------|------|------|------|------|------|
  |      |      |      |      |      |      |      |      |
  |      |      |      |      |      |      |      |      +-- Ascending Node
  |      |      |      |      |      |      |      |
  |      |      |      |      |      |      |      +--- WS_1_SETTING[3]
  |      |      |      |      |      |      |
  |      |      |      |      |      |      +--- South Pole
  |      |      |      |      |      |
  |      |      |      |      |      +--- WS_1_SETTING[2]
  |      |      |      |      |
  |      |      |      |      +--- Descending Node
  |      |      |      |
  |      |      |      +--- WS_1_SETTING[1]
  |      |      |
  |      |      +--- Nord Pole
  |      | 
  |      +--- WS_1_SETTING[0] or WS_1_SETTING[4]
  |
  +--Ascending Node
  


   */

  int idx=-1;
  UINT16 i;
  if(OrbitOffset_ms > PRH_VAR_WS_TIME_ORBIT) {
    *duration_run_ms=PRH_VAR_RM_TIME_MAX_RUN;
    *acq_mode      =RM_ACQ_SETTING_B;
    *effective_ws  =WS_0;
  }else{

    for(i=0;i<PRH_ARR_N_WS_1_SETTING;i++)
      if(OrbitOffset_ms >= PRH_ARR_WS_1_SETTING[i])
	idx=i;
    
    idx++;
    if(idx >= PRH_ARR_N_WS_1_SETTING) {
      *duration_run_ms=PRH_VAR_RM_TIME_MAX_RUN;
      *acq_mode      =RM_ACQ_SETTING_B;
    }else{
      // idx now is the next "transition point"
      *duration_run_ms=PRH_ARR_WS_1_SETTING[idx] - OrbitOffset_ms;
      *acq_mode = idx % 2 == 0 ? RM_ACQ_SETTING_A : RM_ACQ_SETTING_B;
    }
    *effective_ws = WS_1;
  }
  return CM_RC_SUCCESSFUL;
}

/*****************************************************************************/
/* @Function: WS_SetEffectiveWorkingSchedule                                 */
/* @Purpose :                                                                */
/*  set che effective working schedule based on the favourite working        */
/*  schedule. tipically done at the Ascending Node                           */
/*  Also returns the acq_setting and the time_run (always)                   */
/* @@                                                                        */
/* @Parameter Name       @Message                                            */
/*  status_code          OUT     Return code                                 */
/* @@                                                                        */
/*****************************************************************************/

status_code WS_SetEffectiveWorkingSchedule(TI_TIME time_max_run_ms,
					   RM_ACQ_SETTING *acq_mode,
					   TI_TIME *duration_run_ms) {

#define WS_CASE_WS0 do { \
   WS_EffectiveWorkingSchedule = WS_0; \
   result_acq_mode=RM_ACQ_SETTING_B; \
   result_duration_run_ms=time_max_run_ms; \
} while(0)

#define WS_CASE_WS1 do { \
   if(TI_piGetOrbitOffset_s(&OrbitOffset_s) != TI_SUCCESSFUL) { \
      WS_CASE_WS0; \
   }else{ \
      WS_1_GetAcqSetting_and_time(OrbitOffset_s*1000, \
				  &result_acq_mode, \
				  &result_duration_run_ms, \
				  &WS_EffectiveWorkingSchedule); \
   } \
} while(0)

  TI_TIME unused,now_s;
  TI_TIME OrbitOffset_s;
  RM_ACQ_SETTING result_acq_mode;
  TI_TIME        result_duration_run_ms,gap_s;
  BOOL avail_time;
  UINT16 table,idx;
  status_code s;
  switch(WS_FavouriteWorkingSchedule) {
  case WS_1:
    /* chech if the timing of the orbit is available */
    WS_CASE_WS1;
    break;
  case WS_2:
    /* check if the TimeSync is available */
    if(! (TI_piTimeSyncIsAvailable(&avail_time) == TI_SUCCESSFUL && avail_time)) {
      /* if the timesync is not available, do the same of WS1-case */
      WS_CASE_WS1;
    }else{
      if(TI_piGetCurrentMoscowTime_s(&now_s) == TI_SUCCESSFUL) {
	/* delete old tables: */
	WS_2_ClearOldTables(now_s);

	if(WS_2_LookForValidTable(now_s,&table) == CM_RC_SUCCESSFUL) {
	  if(WS_2_LookForValidInfo(now_s,table,&idx,&result_acq_mode,&result_duration_run_ms) == CM_RC_SUCCESSFUL) {
	    WS_EffectiveWorkingSchedule = WS_2;
	    /*@LOG going to WS_2: use this table number */
	    LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,table);
	    /*@LOG going to WS_2: use this entry now... */
	    LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,idx);
	  }else
	    WS_CASE_WS1;
	}
	else
	  WS_CASE_WS1;

	/* check if im in a gap between some table: in such a case the duration (if < of the gap) 
	   must be forced to the gap itself in order to dont loose some table */
	if( WS_EffectiveWorkingSchedule != WS_2 && 
	    WS_2_GetNextUsefulTableGap(now_s,&gap_s) == CM_RC_SUCCESSFUL &&
	    gap_s <= result_duration_run_ms/1000 ) {
	  result_duration_run_ms=gap_s*1000;
	}
	

      }else{
	WS_CASE_WS1;
      }
    }
    break;
  case WS_3:
    WS_EffectiveWorkingSchedule = WS_3;
#warning "set result_acq_mode for WS_3"
    result_acq_mode=WS_3_NextAcqMode;
    result_duration_run_ms=time_max_run_ms; 
    break;
  default:
    WS_CASE_WS0;
    break;
  } // switch
  *acq_mode=result_acq_mode;
  *duration_run_ms=result_duration_run_ms;
  /*@LOG result_acq_mode (A=1/B=2) */
  LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,result_acq_mode);
  /*@LOG result_duration_run_ms */
  LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,result_duration_run_ms);
  return CM_RC_SUCCESSFUL;
}






/*****************************************************************************/
/* @Function: WS_SetFavouriteWorkingSchedule                                 */
/* @Purpose :                                                                */
/*  Set the Favourite Working Schedule.                                      */
/* @@                                                                        */
/* @Parameter Name       @Message                                            */
/*  status_code          OUT     Return code                                 */
/* @@                                                                        */
/*****************************************************************************/


status_code WS_SetFavouriteWorkingSchedule(WS newws) {
  if(WS_FavouriteWorkingSchedule != newws) {
    WS_FavouriteWorkingSchedule = newws;
    PRH_VAR_WS_FAVOURITE_WS = newws;
    //PRH_int_var_write2eeprom(PRH_VAR_WS_FAVOURITE_WS_IDX);
    /*@LOG SetFavourite Working Schedule - newws */
    LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,newws);
  }
  return CM_RC_SUCCESSFUL;
}


status_code WS_3_SetNextAcqMode(RM_ACQ_SETTING mode) {
  WS_3_NextAcqMode = mode;
  /*@LOG WS_3_SetNextAcqMode - WS_3_NextAcqMode (A/B)*/
  LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_3_NextAcqMode);
  return CM_RC_SUCCESSFUL;
}