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

 * File: SharedMemory.cpp

 *--------------------

 * description: this module implements some common shared memory functions 

 *              to facilliate communication between the background and realtime

 *              process of the ARM test bed.  See SharedMemory.h interface for

 *              details

 * author: M. Zinn	

 * date:   2/1/2001	

 *      

 * revision history:

 * 7/24/2003 - P. Thaulad (migrated to QNX 6.0) in preparation for the new ARM

 * project

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



/*-------------------Include Files-------------------------------------------*/

#include <sys/mman.h>

#include <signal.h>

#include <stdio.h>

#include <unistd.h>

#include <stdlib.h>

#include <fcntl.h>

#include <pthread.h>
#include <process.h>

#include <stddef.h>

#include <stdarg.h>

#include "matrix/PrVector.h"

#include "matrix/PrVector3.h"

#include "matrix/PrMatrix.h"

#include "memory.h"

#include "SharedMemory.h"

#include "Definitions.h"





/*-------------------Module Defines------------------------------------------*/

#define MEMFILE			"shared_memory_file"

#define MEMFILE_RT		"RT_shared_memory_file"

#define MEMFILE_DATA		"data_logging_file"

#define OFLAGS			O_CREAT|O_RDWR

#define PERMISSION		0777

#define PROT			PROT_READ|PROT_WRITE

#define MFLAGS			MAP_SHARED





/*-------------------Module Variables----------------------------------------*/

static int fd ,fd_RT,fd_DATA;

static pid_t realtime_pid, background_pid;



//#define TEST_SM





// create a new pointer to the class object sharedMemory

static sharedMemory *SM = new sharedMemory;	



sharedMemory* SM_Functions::Create_Shared_Memory (void)

{

	void *addr;


	shm_unlink(MEMFILE);	// remove the memory object first if it exists


	// create a new memory object

	fd = shm_open(MEMFILE, O_RDWR | O_CREAT, PERMISSION);


	if(fd == -1)

	{

		perror("shared memory creation failed");

		exit(1);

	}



	// set the memory object's size

	if(ftruncate(fd, sizeof(*SM)) == -1)

	{

		perror("shared memory truncation failed");

		exit(1);

	}



	// map the memory object and return a pointer to the object

//	addr = mmap(NULL, sizeof(*SM), PROT_READ|PROT_WRITE, MAP_SHARED, fd,NULL);

	addr = mmap((void*)0x80204000, sizeof(*SM), PROT_READ|PROT_WRITE, MAP_SHARED, fd,NULL);



	if(addr == MAP_FAILED)

	{

		perror("shared memory mapping failed");

		exit(1);

	}



	printf("SM address: %6.6x \n", addr);



	SM = (sharedMemory*)addr;	

	// set the class pointer to the memory object pointer

	// SM now points to a class object sharedMemory that resides 

	// in the shared memory space



	printf("Shared Memory Created \n");

	return(SM);

}



void SM_Functions::Open_Shared_Memory(void)

{

	if((fd = shm_open(MEMFILE, O_RDWR, PERMISSION)) == -1)

	{

		perror("Error opening existing SM");

		exit(1);

	}



	printf("RT opened existing shared memory successfully \n");

}



//sharedMemory* SM_Functions::Map_Shared_Memory(void)

sharedMemory* SM_Functions::Map_Shared_Memory(int address)

{

	void* addr;



	// map the memory object and return a pointer to the object

//	addr = mmap(NULL, sizeof(*SM), PROT_READ|PROT_WRITE, MAP_SHARED, fd,NULL);

//	addr = mmap((void*)0x80204000, sizeof(*SM), PROT_READ|PROT_WRITE, MAP_SHARED, fd,NULL);

	addr = mmap((void*)address, sizeof(*SM), PROT_READ|PROT_WRITE, MAP_SHARED, fd,NULL);



	if(addr == MAP_FAILED)

	{

		perror("shared memory mapping failed");

		exit(1);

	}



	SM = (sharedMemory*)addr;	



	printf("SM address: %6.6x \n", addr);



	return(SM);	// return sharedMemory pointer

}



void SM_Functions::Release_Shared_Memory(void)

{

	munmap(SM, sizeof(*SM));



	close(fd);



	if(shm_unlink(MEMFILE) == -1)

	{

		perror("shared memory closing failed");

		exit(1);

	}



	printf("Shared memory released \n");

}





void SM_Functions::Init_Shared_Memory(void)

{

	int i;
	SM->compliance_direction = 1;	// init compliance direction in OS HOLD to y
	SM->exit_flag = 0;
	SM->userExit = 0;
	SM->userMode = IDLE;
	SM->miniMode = FALSE;
	SM->time = 0.0;
	for(i=0;i<NOJ;i++)

	{
		SM->q[i] = 0.0;

		SM->qinc[i] = 0.0;

		SM->dq[i] = 0.0;

		SM->qdes[i] = SM->q[i];

		SM->tau[i] = 0.0;

		SM->tau_des[i] = 0.0;

		SM->jgoal[i] = 0.0;
	}

	for(i=0;i<3;i++)

	{
		SM->F[i] = 0.0;
		SM->Fstar[i] = 0.0;
		SM->x[i] = 0.0;
		SM->x_des[i] = 0.0;
		SM->xgoal[i] = 0.0;
		SM->desiredForce[i] = 0.0;
	}

	for(i=0;i<6;i++)
	{
		SM->force[i] = 0.0;
		SM->forcedot[i] = 0.0;
	}

//	SM->A = new PrMatrix(5,5);
//	SM->A->zero();

}



void SM_Functions::SM_Functions::Write_To_SM(sharedMemory* localSM)

{

	*SM = *localSM;

	// SM is dereferenced and content of localSM copied to it

}



sharedMemory* SM_Functions::Init_Local_SM (sharedMemory* localSM)

{

	localSM = SM;

	// SM is the pointer to the class object sharedMemory residing

	// in the shared memory space	



	return(localSM);

}



sharedMemory* SM_Functions::Read_SM (sharedMemory* localSM)

{

	localSM = SM;

	return(localSM);

}



#ifdef TEST_SM



void main (void)

{

	sharedMemory* rt;



	SM_Functions sm_functions;

	

	sm_functions.Create_Shared_Memory();



	sm_functions.Init_Shared_Memory();



	printf("%d, %d, %d \n", SM->state, SM->inputKB, SM->inputToggle);



	printf("%6.6x, %d \n", SM, sizeof(*SM));



	SM->rt_state = 1;

	SM->ad_voltage[0] = 4.0;



	printf("RT ad voltage 0: %f \n",SM->ad_voltage[0]);



	printf("RT state: %d \n", SM->rt_state);



	rt = sm_functions.Init_Local_SM(rt);



	rt = SM;



	printf("rt %d here, SM %d \n", rt, SM);



	rt->ad_voltage[0] = 3.0;



	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);



	sm_functions.Write_To_SM(rt);



	printf("RT ad voltage 0: %f \n",SM->ad_voltage[0]);



	sm_functions.Release_Shared_Memory();



}



#endif











	

