#include <stdio.h>
//http://forums.deltatau.com/showthread.php?tid=24 ->examples to write directly to shared memory
#include "RtGpShm.h" // Global Rt/Gp Shared memory pointers
#include "pRtGpShm.h" // Global Rt/Gp Shared memory pointers
//extern "C"
//{
  void mydbg(char* s, int a, int b);
//}
main()
{
  int i;
  int status;
  char buf[256];
  printf("hello\n");
  printf("there\n");
  InitLibrary();
  for (i=0;i<256;i++)
  {
    printf("hello %d\n",i);
    snprintf(buf,sizeof(buf),"Gate3[0].GpioData[0].16.8=%d\n",i);
    status=Command(buf); //ppmac gplib function
    printf("%s ->%d\n",buf,status);
    //mydbg("gugus", i, 2*i);
    usleep(10000);
  }
  {
    int i;
    struct SHM *p;  // Pointer to shared memory
    p=pshm;
    printf("Sys.ServoCtrl 0x%p\n",p->ServoCtrl);

    //p->Motor[4].Servo.Kp=123.f;
    printf("Motor[4].Servo.Kp %f\n",p->Motor[4].Servo.Kp);

    printf("Motor[4].Ctrl 0x%p\n",p->Motor[4].Ctrl);

    printf("UserAlgo.ServoCtrlName[4] %s\n",p->UserAlgo.ServoCtrlName[4]);
    printf("UserAlgo.ServoCtrlAddr[4] %x\n",p->UserAlgo.ServoCtrlAddr[4]);

    printf("Motor[4].UserPhase 0x%p\n",p->Motor[4].UserPhase);
    printf("Motor[4].UserServo 0x%p\n",p->Motor[4].UserServo);


    //for(i=0;i<8;i++)
    //{
    //  printf("UserAlgo.ServoName %s\n",p->UserAlgo.ServoName[i]);
    //  printf("UserAlgo.ServoAddr %x\n",p->UserAlgo.ServoAddr[i]);
    //  printf("UserAlgo.ServoCtrlName %s\n",p->UserAlgo.ServoCtrlName[i]);
    //  printf("UserAlgo.ServoCtrlAddr %x\n",p->UserAlgo.ServoCtrlAddr[i]);
    //}

    strcpy(p->UserAlgo.ServoCtrlName[4],"user_pid_ctrl");
    p->UserAlgo.ServoCtrlAddr[4]= 0xa35e012c;


    //p->Motor[4].Ctrl=0x123456;
    //UserAlgo
  }

  CloseLibrary();
}
