/* 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;
			}
		}
	}

}
