/* Example of SocketCAN CANopen Library for PISO-CAN200/400

   Author: Golden Wang

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software Foundation,
   Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */

/* File level history (record changes for this file here.)

   v 0.0.0 22 Apr 2010 by Golden Wang
     create, blah blah... */

#include "pisocpm.h"

#define kbhit()   (int)getchar()
char Kbhit(void);

char Kbhit(void)
{
  int n=0;

  ioctl(0,FIONREAD,&n);
  if(n) return kbhit();

  return n;
}

int main(void)
{
  int ret;
  int board, port, baud, init = 0;
  struct can_filter rfilter;
  cpmchannel_t channel;
  char command;
 
  printf("\r\n");
  printf("/***************************/\r\n");
  printf("/* Driver  Version = %s */\r\n",CPM_GetDriverVersion());
  printf("/* Library Version = %s */\r\n",CPM_GetLibraryVersion());
  printf("/***************************/\r\n");
  
  printf("CAN Card Board Number(1~%d) :", MAX_CARD_SUPPORT);
  ret = scanf("%d",&board);

  printf("CAN Card Port Number(1~4) :");
  ret = scanf("%d",&port);
  getchar();

  ret = CPM_Open(board, port);

  if (ret){
    printf("Open CAN device failure, Error Code = %d\r\n",ret);
    return 0;
  }

  ret = CPM_InitPort(board, port, &channel);
  if (ret){
    printf("Initial CAN device port failure, Error Code = %d\r\n",ret);
    return 0;
  }

  rfilter.can_id   = 0;
  rfilter.can_mask = 0;

  ret = CPM_Config(&channel, &rfilter, sizeof(rfilter));
  
  if(ret){
    printf("Configure error, Error Code = %d\r\n",ret);
    return 0;
  }
  else{
    printf("CAN Card Configure OK\r\n");
  }
  
  printf("a. Add Node:\r\n"); 
  printf("b. Remove Node:\r\n");  
  printf("c. Get NMT state:\r\n"); 
  printf("d. Change NMT state:\r\n");
  printf("e. Set NMT Guarding:\r\n");
  printf("f. Change EMCY ID:\r\n");
  printf("g. Send SYNC:\r\n");    
  printf("h. ReadSDO:\r\n");  
  printf("i. WriteSDO:\r\n"); 
  printf("j. Install PDO:\r\n");  
  printf("k. Remove PDO:\r\n"); 
  printf("l. Remote PDO:\r\n"); 
  printf("m. Write PDO:\r\n"); 
  printf("n. Get CAN Slave Name:\r\n"); 
  printf("o. Show CPM Demo Item:\r\n");
  printf("q. Shutdown and exit:\r\n"); 
    
  while (1){
    if (!init){ 
      ret = CPM_InitMaster(&channel, &rfilter, sizeof(rfilter), 1000);

      if (ret){
        printf("CAN Master Init Failure, Error Code = %d\r\n",ret);
      }
      else{
        printf("CAN Master Init OK\r\n");
      }

      init = 1;
    }
  
    command = Kbhit();

    if (command){
      switch (command){
        case 'a':
        {
          int node=0;

          printf("Add node:Press node number(1~127):");
          ret = scanf("%d",&node);
          getchar();
          ret = CPM_AddNode(&channel,node);;
          if (ret){
            printf("AddNode Error Code = %d\r\n",ret);
          }
          else{
            printf("Add Node OK\r\n");
          }
        }
        break;

        case 'b':
        {
          int node=0;
        
          printf("Remove node:Press node number(1~127):");
          ret = scanf("%d",&node);
          getchar();
          ret = CPM_RemoveNode(&channel,node);
          if (ret){
            printf("RemoveNode Error Code = %d\r\n",ret);
          }
          else{
            printf("RemoveNode OK\r\n");
          }
        
        }
        break;

        case 'c':
        {

          int node = 0;
          BYTE state;
        
          printf("Get NMT State:Press node number(1~127):");
          ret = scanf("%d",&node);
          getchar();

          ret = CPM_NMTGetState(&channel, node, &state);
        
          if (ret){
            printf("Get NMT State Error Code = %d\r\n",ret);
          }
          else{
            printf("GetState OK: state is [%x] ",state);

            switch (state){
              case 0:
                printf("Initialising Mode\r\n");
                break;
              case 4:
                printf("Stop Mode\r\n");
                break;
              case 5:
                printf("Operational Mode\r\n");
                break;
              case 127:
                printf("Pre-Operational Mode\r\n");
                break;
              default:
                printf("UnKnown Mode\r\n");                  
                break; 
            }

          }
        }
        break;
  
        case 'd':
        {
          int node=0,state=0;
          printf("Change NMT State:Press node number(0~127, node 0 means all slaves):");
          ret = scanf("%d",&node);
        
          printf("1:Operational mode\r\n");
          printf("2:Stop mode\r\n");
          printf("128:Pre-Operational mode\r\n");
          printf("129:Reset Slave\r\n");
          printf("Change State:Press state:");
          ret = scanf("%d",&state);
          getchar();

          ret = CPM_NMTChangeState(&channel, node, state);
          
          if (ret){
            printf("ChangeState Error Code = %d\r\n",ret);
          }
          else{
            printf("ChangeState OK\r\n");
          }
        }
        break;

        case 'e':
        {

          int node=0,life=0,guard=0;
        
          printf("Guarding:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Guarding:Press guard time(0~65535 ms):");
          ret = scanf("%u",&guard);
          printf("Guarding:Press life time(0~255):");
          ret = scanf("%d",&life);
          getchar();
          ret = CPM_NMTGuarding(&channel, node, guard, life, 1000);

          if (ret){
            printf("Guarding Error Code = %d\r\n",ret);
          }
          else{
            printf("Guarding OK\r\n");
          }
        }
        break;

        case 'f':
        {
          int node=0,cobid=0;
        
          printf("Change EMCY ID:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Change EMCY ID:Press EMCY COB ID(hex):");
          ret = scanf("%x",&cobid);
          getchar();
          ret = CPM_ChangeEMCYID(&channel, node, cobid);
          if(ret){
            printf("Change EMCY ID Error Code = %d\r\n",ret);
          }
          else{
            printf("Change EMCY ID OK\r\n");
          }
        }
        break;

        case 'g':
        {
          int cyclic=0,cobid=0, timer = 0;
        
          printf("SYNC:Press SYNC COB ID(hex):");
          ret = scanf("%x",&cobid);

          printf("SYNC:Press cyclic timer(0~65535 ms):");
          ret = scanf("%u",&timer);

          getchar();
        
          ret = CPM_SendSYNC(&channel, cobid, timer);

          if (ret){
            printf("SendSYNC Error Code = %d\r\n",ret);
          }
          else{
            printf("SendSYNC OK\r\n");
          }
        }
        break;

        case 'h':
        {
          int i, j = 0;
          int node=0,index=0,subindex=0;
          DWORD RSize;
          canmsg_t RData;
          BYTE temp[20];

          printf("Read SDO:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Read SDO:Press index(hex):");
          ret = scanf("%x",&index);
          printf("Read SDO:Press sub-index(hex):");
          ret = scanf("%x",&subindex);
          getchar();

          ret = CPM_SDOReadData(&channel, node, index, subindex, &RData, &RSize, 0);

          if (ret == CPM_READ_SEGMENT){
            printf("SDO Segment Data : ");
            do{
              ret = CPM_SDOReadSegment(&channel, node, &RData);
              for (i=0; i<8; i++){
                printf("[%02X]",RData.data[i]); 
              }

            }while (ret == CPM_READ_SEGMENT);
            printf("\r\n");
          }
          else{
            if (ret != CPM_NOERROR && ret != CPM_READ_SEGMENT && ret != CPM_READ_BLOCK){
              printf("ReadSDO Error Code = %d\r\n",ret);
            }
            else{
              printf("SDO data: ");
              for (i=0;i<8;i++){
                printf("[%02X]",RData.data[i]);
              }
              printf("\r\n");
            }
          }
        }
        break;

        case 'i':
        {
          int i;
          int node=0, len=0, index=0, subindex=0;
          DWORD RSize;
          canmsg_t TData, RData;
          unsigned char tdata[40];
        
          printf("Write SDO:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Write SDO:Press index(hex):");
          ret = scanf("%x",&index);
          printf("Write SDO:Press sub-index(hex):");
          ret = scanf("%x",&subindex);
          printf("Write SDO:data length and data(hex)(len,d0,d1,d2,d3):");
          ret = scanf("%d,%hhx,%hhx,%hhx,%hhx",&len,&tdata[0],&tdata[1],&tdata[2],&tdata[3]);
          getchar();
          
          ret = CPM_SDOWriteData(&channel, node, index, subindex, len, &RData, tdata, 0);

          if (ret){
            printf("WriteSDO Error Code = %d\r\n",ret);
          }
          else{
            printf("Write SDO data OK.\r\n");
          }
        }
        break;  

        case 'j':
        {
          unsigned char tdata[40];
          int node=0,cobid=0,txrxtype=0,pdoch=0,ioch=0,io=0;
        
          printf("Install PDO:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Install PDO:Press COB ID(hex):");
          ret = scanf("%x",&cobid);
          printf("Install PDO:Press tx/rx type(0:RxPDO, 1:TxPDO):");
          ret = scanf("%x",&txrxtype);
          if (txrxtype){
            printf("Install PDO:Press AI/DI type(0:AI, 1:DI):");
            ret = scanf("%x",&io);
            if(io){
              tdata[0] = 0x08;
              tdata[2] = 0x00;
              tdata[3] = 0x60;
            }
            else{
              tdata[0] = 0x10;
              tdata[2] = 0x01;
              tdata[3] = 0x64;
            }
          }
          else{
            printf("Install PDO:Press AO/DO type(0:AO, 1:DO):");
            ret = scanf("%x",&io);
            if(io){
              tdata[0] = 0x08;
              tdata[2] = 0x00;
              tdata[3] = 0x62;
            }
            else{
              tdata[0] = 0x10;
              tdata[2] = 0x11;
              tdata[3] = 0x64;
            }
          }
        
          printf("Install PDO:Press the nth input or output channel want to mape:");
          ret = scanf("%d",&ioch);
          tdata[1] = ioch;
        
          printf("Install PDO:Press the nth PDO application object to be mapped:");
          ret = scanf("%d",&pdoch);
          getchar();

          ret = CPM_MappingPDO(&channel, node, txrxtype, cobid, pdoch, tdata);
          if (ret){
            printf("Install PDO Error Code = %d\r\n",ret);
          }
          else{
            ret = CPM_InstallPDO(&channel, node, txrxtype, cobid, 255, 0, 0, 1);
            if(ret){
              printf("Install PDO Error Code = %d\r\n",ret);
            }
            else{
              printf("Install PDO OK\r\n");
            }
          }
        }
        break;        

        case 'k':
        {
          int node=0,cobid=0;

          printf("Remove PDO:Press node number(1~127):");
          ret = scanf("%d",&node);
          printf("Remove PDO:Press COB ID(hex):");
          ret = scanf("%x",&cobid);
          getchar();

          ret = CPM_RemovePDO(&channel, node, cobid);

          if (ret){
            printf("RemovePDO Error Code = %d\r\n",ret);
          }
          else{
            printf("Remove OK\r\n");
          }
        }
        break;

        case 'l':
        {
          int i;
          unsigned char rdata[40];
          unsigned char len=0;
          canmsg_t RData;
          int cobid=0;
        
          printf("Remote PDO:Press COB ID(hex):");
          ret = scanf("%x",&cobid);
          getchar();

          ret = CPM_RemotePDO(&channel, cobid, &RData);

          if(ret){
            printf("RemotePDO Error Code = %d\r\n",ret);
          }
          else{
            printf("Remote data: ");
            for (i=0;i<RData.can_dlc;i++){
            //for(i=0;i<8;i++)
              printf("[%02x]",RData.data[i]);
            }
            printf("\r\n");
            printf("RemotePDO OK\r\n");
                                        }
        }
        break;

        case 'm':
        {
          unsigned char rdata[40];
          int cobid=0,len=0,offset=0;
        
          printf("Write PDO:Press COB ID(hex):");
          ret = scanf("%x",&cobid);
          printf("Write PDO:Press data offset:");
          ret = scanf("%d",&offset);
          printf("Write PDO:Press data length:");
          ret = scanf("%d",&len);
          printf("Write PDO:Press data (d0,d1,d2,d3,d4,d5,d6,d7):");
          ret = scanf("%hhx,%hhx,%hhx,%hhx,%hhx,%hhx,%hhx,%hhx",&rdata[0],&rdata[1],&rdata[2],&rdata[3],&rdata[4],&rdata[5],&rdata[6],&rdata[7]);
          getchar();

          ret = CPM_WritePDO(&channel, cobid, rdata, offset, len);
          if(ret){
            printf("WritePDO Error Code = %d\r\n",ret);
          }
          else{
            printf("WritePDO OK\r\n");
          }
        }
        break;

        case 'n':
        {
          int node=0,ret;
          unsigned char slave_name[20];
          printf("Read SDO:Press node number(1~127):");
          ret = scanf("%d",&node);
          getchar();

          ret = CPM_ReadManufacturerName(&channel, node,slave_name);

          if (ret){
            printf("Test Function Error Code = %d\r\n",ret);
          }
          else{
            printf("CAN Slave is %s \r\n", slave_name);
          }
        }
        break;

        case 'o':
        {
          printf("a. Add Node:\r\n");     
          printf("b. Remove Node:\r\n");  
          printf("c. Get NMT state:\r\n");
          printf("d. Change NMT state:\r\n"); 
          printf("e. Set NMT Guarding:\r\n");
          printf("f. Change EMCY ID:\r\n");
          printf("g. Send SYNC:\r\n");    
          printf("h. ReadSDO:\r\n");     
          printf("i. WriteSDO:\r\n");     
          printf("j. Install PDO:\r\n");  
          printf("k. Remove PDO:\r\n");   
          printf("l. Remote PDO:\r\n"); 
          printf("m. Write PDO:\r\n"); 
          printf("n. Get CAN Slave Name:\r\n"); 
          printf("o. Show CPM Demo Item:\r\n");
          printf("q. Shutdown and exit:\r\n"); 
          getchar();
        }
        break;

        case 'q':
        {
          ret = CPM_ShutdownMaster(&channel);
          if (ret){
            printf("ShutdownMaster Error Code = %d\r\n",ret);
          }
          else{
            printf("ShutdownMaster OK\r\n");
            return 0;
          }
        }
        break;
        default:
          printf("Unknow command = %X\r\n",command);
        break;
      }
    }
  }
}
