Switch statement jumps from one case to another - switch-statement

This is a part of a project for MSP430 microcontroller. My problem is that this switch statement jumps from case 1: (and case 2: as well) to case 11: with no apparent reason. I tried debugging it, the switch value selected_cmd is not changing during the process. The variable selected_cmd is a global variable, its value is set in another function. If I put simple statement selected_cmd = 2 before this switch statement, it works as expected. If selected_cmd is set programmatically, it jumps from one case to another. How to solve this?
switch (selected_cmd)
{
case 0: // set light level
{
DALI_MsgBuf[1] = value;
DALI_FF = DALI_MsgBuf[1] | (DALI_MsgBuf[0] << 8);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 1: // turn off
{
DALI_FF = TURN_OFF | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 2: // recall max
{
DALI_FF = RECALL_MAX_LEVEL | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 3: // recall min
{
DALI_FF = RECALL_MIN_LEVEL | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 4: // store max lvl
{
DALI_FF = (DATA_TRANSFER_REGISTER << 8) | value;
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_FF = STORE_THE_DTR_AS_MAX_LEVEL | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 5: // store min lvl
{
DALI_FF = (DATA_TRANSFER_REGISTER << 8) | value;
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_FF = STORE_THE_DTR_AS_MIN_LEVEL | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 6: // set fade rate
{
DALI_FF = (DATA_TRANSFER_REGISTER << 8) | value;
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_FF = STORE_THE_DTR_AS_FADE_RATE | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 7: // set fade time
{
DALI_FF = (DATA_TRANSFER_REGISTER << 8) | value;
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_FF = STORE_THE_DTR_AS_FADE_TIME | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 8: // add to group
{
DALI_FF = (ADD_TO_GROUP_0 | value) | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 9: // remove from group
{
DALI_FF = (REMOVE_FROM_GROUP_0 | value) | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 10: // store as scene
{
DALI_FF = STORE_ACTUAL_LEVEL_IN_THE_DTR | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_FF = (STORE_THE_DTR_AS_SCENE_0 | value) | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 11: // go to scene
{
DALI_FF = (GO_TO_SCENE_0 | value) | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 12: // remove from scene
{
DALI_FF = (REMOVE_FROM_SCENE_0 | value) | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
case 13: // reset
{
DALI_FF = RESET | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
}
default: break;
}

Use
switch (selected_cmd)
{
case 0: // set light level
DALI_MsgBuf[1] = value;
DALI_FF = DALI_MsgBuf[1] | (DALI_MsgBuf[0] << 8);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
case 1: // turn off
DALI_FF = TURN_OFF | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
case 2: // recall max
DALI_FF = RECALL_MAX_LEVEL | ((DALI_MsgBuf[0] << 8) | 0x01);
DALI_Transmit(DALI_FF);
timer_sleep_ms(5);
break;
...
Do not use brace blocks after a case: statement
Do you call this function from an interrupt. Then the debugger might get confused when a new interupt enters the method again.

Related

Converting a AfxMessageBox into a CTaskDialog using DoMessageBox

I have written this function so far:
int CMFCApplication3App::DoMessageBox(LPCTSTR lpszPrompt, UINT nType, UINT nIDPrompt)
{
CString strContent = CString(lpszPrompt);
CString strTitle; strTitle.LoadString(AFX_IDS_APP_TITLE);
CTaskDialog dlgTaskMessageBox(strContent, _T(""), strTitle);
int iPixelWidth = (::GetSystemMetrics(SM_CXSCREEN) / 100) * 30;
int iDialogUnitsWidth = MulDiv(iPixelWidth, 4, LOWORD(GetDialogBaseUnits()));
dlgTaskMessageBox.SetDialogWidth(iDialogUnitsWidth);
/*
if (nType & MB_ICONINFORMATION)
dlgTaskMessageBox.SetMainIcon(TD_INFORMATION_ICON);
if (nType & MB_ICONERROR)
dlgTaskMessageBox.SetMainIcon(TD_ERROR_ICON);
if (nType & MB_ICONWARNING)
dlgTaskMessageBox.SetMainIcon(TD_WARNING_ICON);
if (nType & MB_ICONQUESTION)
{
HICON hIcon = LoadIcon(IDI_QUESTION);
dlgTaskMessageBox.SetMainIcon(hIcon);
}
int iButtons = 0;
if (nType & IDYES)
iButtons |= TDCBF_YES_BUTTON;
if (nType & IDNO)
iButtons |= TDCBF_NO_BUTTON;
if (nType & IDCANCEL)
iButtons |= TDCBF_CANCEL_BUTTON;
if (nType & IDOK)
iButtons |= TDCBF_OK_BUTTON;
if (nType & IDRETRY)
iButtons |= TDCBF_RETRY_BUTTON;
dlgTaskMessageBox.SetCommonButtons(iButtons);
*/
if (nType == (MB_YESNOCANCEL | MB_ICONERROR))
{
dlgTaskMessageBox.SetCommonButtons(TDCBF_YES_BUTTON | TDCBF_NO_BUTTON | TDCBF_CANCEL_BUTTON);
dlgTaskMessageBox.SetMainIcon(TD_ERROR_ICON);
}
if (nType == (MB_YESNOCANCEL | MB_ICONWARNING))
{
dlgTaskMessageBox.SetCommonButtons(TDCBF_YES_BUTTON | TDCBF_NO_BUTTON | TDCBF_CANCEL_BUTTON);
dlgTaskMessageBox.SetMainIcon(TD_WARNING_ICON);
}
if (nType == (MB_YESNOCANCEL | MB_ICONINFORMATION))
{
dlgTaskMessageBox.SetCommonButtons(TDCBF_YES_BUTTON | TDCBF_NO_BUTTON | TDCBF_CANCEL_BUTTON);
dlgTaskMessageBox.SetMainIcon(TD_INFORMATION_ICON);
}
/*
if (nType == (MB_YESNOCANCEL | MB_ICONQUESTION))
{
dlgTaskMessageBox.SetCommonButtons(TDCBF_YES_BUTTON | TDCBF_NO_BUTTON | TDCBF_CANCEL_BUTTON);
HICON hIcon = LoadIcon(IDI_QUESTION);
dlgTaskMessageBox.SetMainIcon(hIcon);
}
*/
return dlgTaskMessageBox.DoModal();
}
I have two issues with this and am happy to split as two questions:
Using IDI_QUESTION is causing the application to crash.
Isn't there an easier way to decode nType into the various buttons and icon required?
Using IDI_QUESTION is causing the application to crash
That's because IDI_QUESTION is a standard icon and must be loaded by passing a NULL instance handle to ::LoadIcon, but MFC's CWinApp::LoadIcon passes AfxGetResourceHandle() instead. The following bypasses MFC and calls the Win32 API directly.
HICON hIcon = ::LoadIcon(NULL, IDI_QUESTION);
Isn't there an easier way to decode nType into the various buttons and icon required?
Not a lot easier, but they could be grouped to lessen the duplication.
int nCommonButtons = 0;
switch(nType)
{
case MB_YESNOCANCEL:
nCommonButtons |= TDCBF_CANCEL_BUTTON;
case MB_YESNO:
nCommonButtons |= TDCBF_YES_BUTTON | TDCBF_NO_BUTTON;
break;
case MB_OKCANCELRETRY:
nCommonButtons |= TDCBF_RETRY_BUTTON;
case MB_OKCANCEL:
nCommonButtons |= TDCBF_CANCEL_BUTTON;
case MB_OK:
nCommonButtons |= TDCBF_OK_BUTTON;
break;
//... etc
}
I thought I would add this as addition answer.
Taking onboard the accepted answer (#dxiv) and the comments that were made below (#sergiol):
int CMeetingScheduleAssistantApp::DoMessageBox(LPCTSTR lpszPrompt, UINT nType, UINT nIDPrompt)
{
CString strContent = CString(lpszPrompt);
CString strTitle = CString();
if (!CTaskDialog::IsSupported())
return CWinAppEx::DoMessageBox(lpszPrompt, nType, nIDPrompt);
ENSURE(strTitle.LoadString(AFX_IDS_APP_TITLE));
CTaskDialog dlgTaskMessageBox(strContent, _T(""), strTitle);
int iPixelWidth = (::GetSystemMetrics(SM_CXSCREEN) / 100) * 30;
int iDialogUnitsWidth = MulDiv(iPixelWidth, 4, LOWORD(GetDialogBaseUnits()));
dlgTaskMessageBox.SetDialogWidth(iDialogUnitsWidth);
HICON hQuestionIcon = ::LoadIcon(nullptr, IDI_QUESTION);
// Icon
switch (nType & MB_ICONMASK)
{
case MB_ICONERROR:
dlgTaskMessageBox.SetMainIcon(TD_ERROR_ICON);
break;
case MB_ICONWARNING:
dlgTaskMessageBox.SetMainIcon(TD_WARNING_ICON);
break;
case MB_ICONINFORMATION:
dlgTaskMessageBox.SetMainIcon(TD_INFORMATION_ICON);
break;
case MB_ICONQUESTION:
dlgTaskMessageBox.SetMainIcon(hQuestionIcon);
break;
}
// Buttons
int nCommonButtons = 0;
switch (nType & MB_TYPEMASK)
{
case MB_YESNOCANCEL:
nCommonButtons |= TDCBF_CANCEL_BUTTON;
[[fallthrough]];
case MB_YESNO:
nCommonButtons |= TDCBF_YES_BUTTON | TDCBF_NO_BUTTON;
break;
case MB_RETRYCANCEL:
nCommonButtons |= TDCBF_RETRY_BUTTON | TDCBF_NO_BUTTON;
break;
case MB_OKCANCEL:
nCommonButtons |= TDCBF_CANCEL_BUTTON;
[[fallthrough]];
case MB_OK:
default:
nCommonButtons |= TDCBF_OK_BUTTON;
}
dlgTaskMessageBox.SetCommonButtons(nCommonButtons);
return static_cast<int>(dlgTaskMessageBox.DoModal());
}
I was not aware of these:
MB_ICONMASK
MB_TYPEMASK
The only scenario I have not catered for is MB_ABORTRETRYIGNORE because I do not see common buttons equivalents for Abort and Ignore.

Function as left operand - What do I do?

As much as I searched for answers, I can't quite understand any of them, so I decided to ask for direct help.
Basically, this program is supposed to read each digit of the number and then write it down normally. for now this is a 'skeleton' code as I am trying to get the main idea of how to program it further, but I am already stumbling upon errors.
I get an error in whichever line of switch function I use, yet I don't know how to solve it. I get an error error C2659: '=': function as left operand
#include <iostream>
#include <string>
using namespace std;
int number, digit;
int i;
string word[4];
void lowering();
void units();
void tens();
int main()
{
i = 4;
word[i];
cout << "Enter a number: ";
cin >> number;
cout << endl;
lowering();
units();
i--;
lowering();
tens();
for(int x = 4; x >=0; x--)
cout << word[x];
system("pause");
return 0;
}
void lowering()
{
digit = number % 10;
number = number / 10;
}
void units()
{
switch (digit)
{
case 1:
word[i].append = " one"; break;
case 2:
word[i].append = " two"; break;
case 3:
word[i].append = " three"; break;
case 4:
word[i].append = " four"; break;
case 5:
word[i].append = " five"; break;
case 6:
word[i].append = " six"; break;
case 7:
word[i].append = " seven"; break;
case 8:
word[i].append = " eight"; break;
case 9:
word[i].append = " nine"; break;
default:
word[i].append = "";
}
word[i].append = " ";
}
void tens()
{
switch (digit)
{
case 1:
word[i].append = " ten"; break;
case 2:
word[i].append = " twenty "; break;
case 3:
word[i].append = " thirty "; break;
case 4:
word[i].append = " fourty"; break;
case 5:
word[i].append = " fifty"; break;
case 6:
word[i].append = " sixty"; break;
case 7:
word[i].append = " seventy"; break;
case 8:
word[i].append = " eighty"; break;
case 9:
word[i].append = " ninety"; break;
default:
word[i].append = "";
}
word[i].append = " ";
}
In your switch cases, you should use this:
void units()
{
switch (digit)
{
case 1:
word[i].append(" one"); break;
case 2:
word[i].append(" two"); break;
case 3:
word[i].append(" three"); break;
case 4:
word[i].append(" four"); break;
case 5:
word[i].append(" five"); break;
case 6:
word[i].append(" six"); break;
case 7:
word[i].append(" seven"); break;
case 8:
word[i].append(" eight"); break;
case 9:
word[i].append(" nine"); break;
default:
word[i].append("");
}
word[i].append(" ");
}
Besides for better code quality, you should put the break; into a new line.

Using State Machine for Controlling Data

I am sending some data from one Atmega644P to another Atmega644P consecutively. The first three bytes are SYN(0x16), DLE(0x10), and STX(0x02). On the receiving part, I made a state machine to control if I receive those bytes correctly.
When it is in DLE_1_s state, it goes directly to the "else" statement, although the data is 0x10. Why is this happening?
I am using Peter Fleury's uart library.
This is the diagram I am trying to implement:
Code:
int main(void)
{
DDRD = 0b11111010; // PORTD input/output.
DDRC = 0xFF;
uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) );
sei();
unsigned int rec_char;
char buffer[7];
while(1)
{
rec_char=uart_getc();
switch(state)
{
case SYN_s:
{
if ((unsigned char) rec_char == 0x16) // SYN
{
state=DLE_1_s;
}
else
{
state=SYN_s;
}
}
break;
case DLE_1_s:
{
if ((unsigned char) rec_char == 0x10) // DLE
{
state=STX_s;
}
else if ((unsigned char) rec_char == 0x16) // SYN
{
state=DLE_1_s;
}
else
{
state=SYN_s;
}
}
break;
case STX_s:
{
if ((unsigned char) rec_char == 0x02) // STX
{
state=TARGET_NO_1_s;
}
else if ((unsigned char) rec_char == 0x16) // SYN
{
state=DLE_1_s;
}
else
{
state=SYN_s;
}
}
break;
#include <stdio.h>
#define FAKE 1
#if FAKE
unsigned DDRD, DDRC;
#define UART_BAUD_RATE 123
#define F_CPU 0
#define UART_BAUD_SELECT(a,b) (((a)<<8)+((b)&0xff))
#define UART_NO_DATA 1
#define SYN 0x16
#define DLE 0x10
#define STX 0x02
extern void uart_init(unsigned);
extern void sei(void);
extern void maybe_sleep(void);
extern unsigned uart_getc(void);
#endif
int main(void)
{
DDRD = 0b11111010; // PORTD input/output.
DDRC = 0xFF;
uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) );
sei();
unsigned int rec_char;
int state;
for(state=0; ; ) {
unsigned int hi,lo;
rec_char = uart_getc();
hi = rec_char >> 8;
lo = rec_char & 0xff;
switch (hi) { /** handle errors from uart here */
case 0: break;
case UART_NO_DATA: /* no receive data available */
maybe_sleep();
continue;
default:
fprintf(stderr, "Hi=%x Lo=%x\n", hi, lo);
/* some cleanup here ... */
state =0;
continue;
}
switch(state) {
case 0:
if (lo == SYN) { state = 1 ; break;}
break;
case 1:
if (lo == DLE) { state = 2; break; }
if (lo == SYN) { break; }
state= 0;
break;
case 2:
if (lo == STX) { state = 3; break; }
if (lo == SYN ) { state = 1; break; }
state = 0;
break;
case 3:
/* handle payload here ... */
break;
}
}
return 0;
}

text section of MFC in memory changes everytime

I am trying to implement self integrity check in MFC exe.
Ref : -
Tamper Aware and Self Healing Code : Codeproject
I dumped text section
ofstream myfile;
myfile.open ("textsec.bin");
myfile.write((const char*)pCodeStart,dwCodeSize);
myfile.close();
every time its different so hash for it is also different. Please Help
I want to check text i.e. code section in memory should be same as that in file present on disk.
So we can be sure that our exe file in memory is not patched.
we can use while distributing product exe.
We will get start of code section in memory by
VOID ImageInformation( HMODULE& hModule, PVOID& pVirtualAddress,
PVOID& pCodeStart, SIZE_T& dwCodeSize,
PVOID& pCodeEnd )
{
const UINT PATH_SIZE = 2 * MAX_PATH;
TCHAR szFilename[ PATH_SIZE ] = { 0 };
__try {
/////////////////////////////////////////////////
/////////////////////////////////////////////////
if( 0 == GetModuleFileName( NULL, szFilename, PATH_SIZE ) )
{
std::tcerr << _T("Error Retrieving Process Filename");
std::tcerr << std::endl;
__leave;
}
hModule = GetModuleHandle( szFilename );
if( NULL == hModule )
{
std::tcerr << _T("Error Retrieving Process Module Handle");
std::tcerr << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_DOS_HEADER pDOSHeader = NULL;
pDOSHeader = static_cast<PIMAGE_DOS_HEADER>( (PVOID)hModule );
if( pDOSHeader->e_magic != IMAGE_DOS_SIGNATURE )
{
std::tcerr << _T("Error - File is not EXE Format");
std::tcerr << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_NT_HEADERS pNTHeader = NULL;
pNTHeader = reinterpret_cast<PIMAGE_NT_HEADERS>(
(PBYTE)hModule + pDOSHeader->e_lfanew );
if( pNTHeader->Signature != IMAGE_NT_SIGNATURE )
{
std::tcerr << _T("Error - File is not PE Format") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_FILE_HEADER pFileHeader = NULL;
pFileHeader = reinterpret_cast<PIMAGE_FILE_HEADER>(
(PBYTE)&pNTHeader->FileHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_OPTIONAL_HEADER pOptionalHeader = NULL;
pOptionalHeader = reinterpret_cast<PIMAGE_OPTIONAL_HEADER>(
(PBYTE)&pNTHeader->OptionalHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
if( IMAGE_NT_OPTIONAL_HDR32_MAGIC !=
pNTHeader->OptionalHeader.Magic )
{
std::tcerr << _T("Error - File is not 32 bit") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_SECTION_HEADER pSectionHeader = NULL;
pSectionHeader = reinterpret_cast<PIMAGE_SECTION_HEADER>(
(PBYTE)&pNTHeader->OptionalHeader +
pNTHeader->FileHeader.SizeOfOptionalHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
const CHAR TEXT[] = ".text";
const CHAR BSSTEXT[] = ".textbss";
UINT nSectionCount = pNTHeader->FileHeader.NumberOfSections;
CHAR szSectionName[ IMAGE_SIZEOF_SHORT_NAME + 1 ];
szSectionName[ IMAGE_SIZEOF_SHORT_NAME ] = '\0';
for( UINT i = 0; i < nSectionCount; i++ )
{
memcpy( szSectionName, pSectionHeader->Name,
IMAGE_SIZEOF_SHORT_NAME );
if( 0 == strncmp( TEXT, szSectionName,
IMAGE_SIZEOF_SHORT_NAME ) )
{
std::tcout << std::endl;
break;
}
pSectionHeader++;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
if( 0 != strncmp( TEXT, szSectionName, IMAGE_SIZEOF_SHORT_NAME ) )
{
std::tcerr << _T("Error - Unable to locate ");
std::cerr << TEXT;
std::tcerr << _T(" TEXT") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pVirtualAddress = (PVOID)(pSectionHeader->VirtualAddress);
dwCodeSize = pSectionHeader->Misc.VirtualSize;
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pCodeStart = (PVOID)(((PBYTE)hModule) +
(SIZE_T)((PBYTE)pVirtualAddress) );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pCodeEnd = (PVOID)((PBYTE)pCodeStart + dwCodeSize );
}
__except( EXCEPTION_EXECUTE_HANDLER ) {
std::tcerr << std::endl << _T("Caught Exception") << std::endl;
}
}
Now we can calculate hash of code section
CalculateImageHash( pCodeStart, dwCodeSize, cbCalculatedImageHash );
then we compare with cbExpectedImageHash. cbExpectedImageHash is predefined global
if( 0 == memcmp( cbExpectedImageHash, cbCalculatedImageHash,
CryptoPP::SHA224::DIGESTSIZE ) )
{
std::tcout << _T("Image is verified.") << std::endl;
}
In MFC cbCalculatedImageHash is every time different . :( but not in console app.
cbExpectedImageHash and cbCalculatedImageHash are globel .
here is full code of my dialog
// UserAppDlg.cpp : implementation file
//
#include "stdafx.h"
#include "UserApp.h"
#include "UserAppDlg.h"
#include "sha.h" // SHA
#include "hex.h" // HexEncoder
#include "files.h" // FileSink
#include "filters.h" // StringSink
#include "gzip.h"
using namespace std;
#ifdef _DEBUG
#define new DEBUG_NEW
#endif
// CAboutDlg dialog used for App About
// we can add cbExpectedImageHash new values if we get calcualted hash same every
BYTE cbExpectedImageHash[ CryptoPP::SHA224::DIGESTSIZE ] =
{
0xBA, 0x71, 0x29, 0xD8, 0x79, 0x4E, 0xA3, 0x5A, 0x2C, 0xE0
, 0xC5, 0x3A, 0x68, 0x9E, 0xE8, 0x29, 0x09, 0x44, 0xFA, 0x71
, 0xAF, 0xDB,0x3E,0x88,0xAD,0x65,0x67,0x78
};
BYTE cbCalculatedImageHash[ CryptoPP::SHA224::DIGESTSIZE ] ;
class CAboutDlg : public CDialog
{
public:
CAboutDlg();
// Dialog Data
enum { IDD = IDD_ABOUTBOX };
protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support
// Implementation
protected:
DECLARE_MESSAGE_MAP()
};
CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
}
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
}
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
END_MESSAGE_MAP()
// CUserAppDlg dialog
CUserAppDlg::CUserAppDlg(CWnd* pParent /*=NULL*/)
: CDialog(CUserAppDlg::IDD, pParent)
{
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}
void CUserAppDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
}
BEGIN_MESSAGE_MAP(CUserAppDlg, CDialog)
ON_WM_SYSCOMMAND()
ON_WM_PAINT()
ON_WM_QUERYDRAGICON()
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
// CUserAppDlg message handlers
int CUserAppDlg::ImageInformation( HMODULE& hModule, PVOID& pVirtualAddress,
PVOID& pCodeStart, SIZE_T& dwCodeSize,
PVOID& pCodeEnd )
{
const UINT PATH_SIZE = 2 * MAX_PATH;
TCHAR szFilename[ PATH_SIZE ] = { 0 };
__try {
/////////////////////////////////////////////////
/////////////////////////////////////////////////
if( 0 == GetModuleFileName( NULL, szFilename, PATH_SIZE ) )
{
// std::tcerr << _T("Error Retrieving Process Filename");
// std::tcerr << std::endl;
__leave;
}
hModule = GetModuleHandle( szFilename );
if( NULL == hModule )
{
// std::tcerr << _T("Error Retrieving Process Module Handle");
// std::tcerr << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_DOS_HEADER pDOSHeader = NULL;
pDOSHeader = static_cast<PIMAGE_DOS_HEADER>( (PVOID)hModule );
if( pDOSHeader->e_magic != IMAGE_DOS_SIGNATURE )
{
// std::tcerr << _T("Error - File is not EXE Format");
// std::tcerr << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_NT_HEADERS pNTHeader = NULL;
pNTHeader = reinterpret_cast<PIMAGE_NT_HEADERS>(
(PBYTE)hModule + pDOSHeader->e_lfanew );
if( pNTHeader->Signature != IMAGE_NT_SIGNATURE )
{
// std::tcerr << _T("Error - File is not PE Format") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_FILE_HEADER pFileHeader = NULL;
pFileHeader = reinterpret_cast<PIMAGE_FILE_HEADER>(
(PBYTE)&pNTHeader->FileHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_OPTIONAL_HEADER pOptionalHeader = NULL;
pOptionalHeader = reinterpret_cast<PIMAGE_OPTIONAL_HEADER>(
(PBYTE)&pNTHeader->OptionalHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
if( IMAGE_NT_OPTIONAL_HDR32_MAGIC !=
pNTHeader->OptionalHeader.Magic )
{
// std::tcerr << _T("Error - File is not 32 bit") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
PIMAGE_SECTION_HEADER pSectionHeader = NULL;
pSectionHeader = reinterpret_cast<PIMAGE_SECTION_HEADER>(
(PBYTE)&pNTHeader->OptionalHeader +
pNTHeader->FileHeader.SizeOfOptionalHeader );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
const CHAR TEXT[] = ".text";
const CHAR BSSTEXT[] = ".textbss";
UINT nSectionCount = pNTHeader->FileHeader.NumberOfSections;
CHAR szSectionName[ IMAGE_SIZEOF_SHORT_NAME + 1 ];
szSectionName[ IMAGE_SIZEOF_SHORT_NAME ] = '\0';
for( UINT i = 0; i < nSectionCount; i++ )
{
memcpy( szSectionName, pSectionHeader->Name,
IMAGE_SIZEOF_SHORT_NAME );
if( 0 == strncmp( TEXT, szSectionName,
IMAGE_SIZEOF_SHORT_NAME ) )
{
// std::tcout << std::endl;
break;
}
pSectionHeader++;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
if( 0 != strncmp( TEXT, szSectionName, IMAGE_SIZEOF_SHORT_NAME ) )
{
// std::tcerr << _T("Error - Unable to locate ");
// std::cerr << TEXT;
// std::tcerr << _T(" TEXT") << std::endl;
__leave;
}
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pVirtualAddress = (PVOID)(pSectionHeader->VirtualAddress);
dwCodeSize = pSectionHeader->Misc.VirtualSize;
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pCodeStart = (PVOID)(((PBYTE)hModule) +
(SIZE_T)((PBYTE)pVirtualAddress) );
/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
pCodeEnd = (PVOID)((PBYTE)pCodeStart + dwCodeSize );
}
__except( EXCEPTION_EXECUTE_HANDLER ) {
// std::tcerr << std::endl << _T("Caught Exception") << std::endl;
}
return 0;
}
void CUserAppDlg::CalculateImageHash( PVOID pCodeStart, SIZE_T dwCodeSize,
PBYTE pcbDigest )
{
CryptoPP::SHA224 hash;
hash.Update( (PBYTE)pCodeStart, dwCodeSize );
hash.Final( pcbDigest );
}
BOOL CUserAppDlg::OnInitDialog()
{
CDialog::OnInitDialog();
// Add "About..." menu item to system menu.
HMODULE hModule = NULL;
PVOID pVirtualAddress = NULL;
PVOID pCodeStart = NULL;
PVOID pCodeEnd = NULL;
SIZE_T dwCodeSize = 0;
ImageInformation( hModule, pVirtualAddress, pCodeStart,
dwCodeSize, pCodeEnd );
ofstream myfile;
myfile.open ("textsec.bin"); //textsec.bin changes everytime
myfile.write((const char*)pCodeStart,dwCodeSize);
myfile.close();
CalculateImageHash( pCodeStart, dwCodeSize, cbCalculatedImageHash ); //cbCalculatedImageHash changes everytime
myfile.open ("CalculateImageHash.bin");
myfile.write((const char*)cbCalculatedImageHash,CryptoPP::SHA224::DIGESTSIZE);
myfile.close();
// IDM_ABOUTBOX must be in the system command range.
ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
ASSERT(IDM_ABOUTBOX < 0xF000);
CMenu* pSysMenu = GetSystemMenu(FALSE);
if (pSysMenu != NULL)
{
CString strAboutMenu;
strAboutMenu.LoadString(IDS_ABOUTBOX);
if (!strAboutMenu.IsEmpty())
{
pSysMenu->AppendMenu(MF_SEPARATOR);
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
}
}
// Set the icon for this dialog. The framework does this automatically
// when the application's main window is not a dialog
SetIcon(m_hIcon, TRUE); // Set big icon
SetIcon(m_hIcon, FALSE); // Set small icon
// TODO: Add extra initialization here
return TRUE; // return TRUE unless you set the focus to a control
}
void CUserAppDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
if ((nID & 0xFFF0) == IDM_ABOUTBOX)
{
CAboutDlg dlgAbout;
dlgAbout.DoModal();
}
else
{
CDialog::OnSysCommand(nID, lParam);
}
}
// If you add a minimize button to your dialog, you will need the code below
// to draw the icon. For MFC applications using the document/view model,
// this is automatically done for you by the framework.
void CUserAppDlg::OnPaint()
{
if (IsIconic())
{
CPaintDC dc(this); // device context for painting
SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);
// Center icon in client rectangle
int cxIcon = GetSystemMetrics(SM_CXICON);
int cyIcon = GetSystemMetrics(SM_CYICON);
CRect rect;
GetClientRect(&rect);
int x = (rect.Width() - cxIcon + 1) / 2;
int y = (rect.Height() - cyIcon + 1) / 2;
// Draw the icon
dc.DrawIcon(x, y, m_hIcon);
}
else
{
CDialog::OnPaint();
}
}
// The system calls this function to obtain the cursor to display while the user drags
// the minimized window.
HCURSOR CUserAppDlg::OnQueryDragIcon()
{
return static_cast<HCURSOR>(m_hIcon);
}

How to make this path finding C++ code into dynamic so that agent can search the path on its own

How to make this path finding C++ code into dynamic so that agent can search the path on its own (from one corner of grid to another corner) but agent can't see unless and until move to the grid. I am new for C++ and try to made this little program but it seems to be static I need help so that I can change it into dynamic or may be agent can do the path search on its own not by assigning the path, so far I made 5 different path, because agent can choose as many path as he want but the path length should be 6 and can move only in up or right direction. Any help will be appreciated.
Thanks in advance
#include <iostream>
#include <ctime>
#include <cstdlib>
#include <iomanip>
#include <math.h>
#include <ctime>
#include <fstream>
#include <vector>
#include <iterator>
#include <algorithm>
using namespace std;
using namespace std;
using namespace System;
using namespace System::IO;
/*int Grid[ 4 ][ 4 ] =
{
{ 29, 8, 11, 9 },
{ 22, 24, 3, 15 },
{ 24, 26, 5, 6 },
{ 33, 10, 32, 21}
};
int Grid[ 4 ][ 4 ] =
{
{ 0, 0, -5, 0.0 },
{ -1, 0, 2, 2 },
{ 2, 0, 0, 0 },
{ 0, 2, 0, -1}
};
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
int g[16] = {33,10,32,21,24,26,5,6,22,24,3,15,29,8,11,9};
int r[16] = {0,2,0,-1,2,0,0,0,-1,0,2,2,0,0,-5,0};
#define SPACE "[ ]"
bool Finished( false );
int m[4];
//int E = 6;
int E, E1, E2, E3;
void printmatrix(char arg[], int length)
{
for (int j = length; j >= 0; j=-4)
{
cout <<"\n" << arg[j] << "\n ";
}
//}
}
void FindNextPos()
{ std::cout<<"\n"<<"Search Path 1"<<"\n";
for (int c= 1; c < 5; c++)
{ if(r[c] != r[3])
{
m[c] = 1;
E = E + r[c] - 1;
//std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E <<")"<<m[c]<< std::endl;
//system("pause");
}
else
{
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E <<")"<<m[c]<< std::endl;
}
}
for (int c= 7; c <= 16; c= c+ 4)
{
if(r[c] != r[16]){
m[c] = 2;
E = E + r[c] - 1;
//std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E <<")"<< std::endl;
//system("pause");
}
else
{
//m[c] = 2;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E <<")"<< std::endl;
}
}
}
//int E1 = 6;
void nextpos1(){
std::cout<<"\n"<<"Search Path 2"<<"\n";
for (int c= 0; c < 3; c++)
{ if(r[c] != r[3])
{
m[c] = 3;
E1 = E1 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 <<")"<< std::endl;
//system("pause");
}
else
{
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 <<")"<< std::endl;
}
}
for (int c= 6; c < 15; c= c + 4)
{
if(r[c] != r[16]){
m[c] = 4;
E1 = E1 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 4;
Finished = true;
std::cout<<"(ID,C,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 + r[c] - 1 <<")"<< std::endl;
}
}
for (int c= 15; c < 16; ++c)
{
if(r[c] != 0){
m[c] = 5;
E1 = E1 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 5;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E1 <<")"<< std::endl;
std::cout << "\n"<<"finish-->" << r[c] << "\t" <<"E1-->"<< E1 <<"\n" ;
}
}
}
//int E2 = 6;
void nextpos2(){
std::cout<<"\n"<<"Search Path 3"<<"\n";
for (int c= 0; c < 2; c++)
{ if(r[c] != r[3])
{
m[c] = 6;
E2 = E2 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E2 <<")"<< std::endl;
//system("pause");
}
else
{
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E2 <<")"<< std::endl;
}
}
for (int c= 1; c < 12; c= c + 4)
{
if(r[c] != r[14]){
m[c] = 7;
E2 = E2 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E2 <<")"<< std::endl;
//system("pause");
}
else
{
//m[c] = 7;
Finished = true;
std::cout<<"(C,E)"<<"-->"<<"("<<r[c]<<","<<E2 + r[c] - 1 <<")"<< std::endl;
}
}
for (int c= 13; c < 16; ++c)
{ m[c] = 8;
if(r[c] != r[16]){
E2 = E2 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E2 <<")"<< std::endl;
//system("pause");
}
else
{
// m[c] = 8;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E2 <<")"<< std::endl;
std::cout << "\n"<<"finish-->" << r[c] << "\t" <<"E-->"<< E1 <<"\n" ;
}
}
}
//int E3 = 6;
void nextpos3(){
std::cout<<"\n"<<"Search Path 4"<<"\n";
for (int c= 0; c <= 12; c= c+ 4)
//for (int c= 4; c <= 12; c= c+ 4)
{ if(r[c] != r[12])
{
m[c] = 11;
E3 = E3 + r[c] - 1;
std::cout<<"\n";
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E3 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 11;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E3 + r[c] - 1 <<")"<< std::endl;
}
}
E3 = E3 -1;
for (int c= 13; c < 16; c++)
{
if(r[c] != r[15]){
m[c] = 12;
E3 = E3 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E3 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 12;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E3 + r[c] - 1 <<")"<< std::endl;
}
}
}
void nextpos4(){
int E4 = 6;
std::cout<<"\n"<<"Search Path 5"<<"\n";
for (int c= 4; c <= 8; c= c+ 4)
//for (int c= 4; c <= 12; c= c+ 4)
{ if(r[c] != r[8])
{ cout<<E4;
m[c] = 13;
E4 = E4 + r[c] - 1;
std::cout<<"\n";
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E4 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 13;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E4 + r[c] - 1 <<")"<< std::endl;
}
}
E4 = E4 -1;
for (int c= 9; c < 12; c++)
{
if(r[c] != r[11]){
m[c] = 14;
E4 = E4 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E4 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 14;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E3 + r[c] - 1 <<")"<< std::endl;
}
}
for (int c= 11; c < 16; c = c + 4)
{
if(r[c] != r[15]){
//m[c] = 15;
E4 = E4 + r[c] - 1;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E4 <<")"<< std::endl;
//system("pause");
}
else
{
m[c] = 15;
Finished = true;
std::cout<<"(ID,R,E)"<<"-->"<<"("<<g[c]<<","<<r[c]<<","<<E4 + r[c] - 1 <<")"<< std::endl;
}
}
}
void PrintRoute( void )
{//std::cout << "Start Node : " << g[0]<<"\t" <<"Finish Node : " << g[15] <<"\n";
std::cout << "\n"<< "Route 1 : "<<"\n";
for (int c= 1; c < 14; c++)
{
if( m[c] == 1)
std::cout<< "[r]-->"<<"\t";
else if (m[c] == 2)
std::cout<< "[u]-->"<<"\t";
else
std::cout <<"";
}
std::cout << "\n";
}
void PrintRoute1( void )
{//std::cout << "\n"<< "Start Node : " << g[0]<<"\t" << "Finish Node : " << g[15]<<"\n";
std::cout << "\n"<< "Route 2 : "<<"\n";
for (int c= 1; c < 16; c++)
{
if( m[c] == 3 )
std::cout<< "[r]-->"<<"\t";
else if (m[c] == 4 )
std::cout<< "[u]-->"<<"\t";
else if (m[c] == 5)
std::cout<<"[r]-->"<<"\t";
else
std::cout <<"";
}
std::cout << "\n";
}
void PrintRoute2( void )
{//std::cout << "\n"<< "Start Node : " << g[0] <<"\t"<< "Finish Node : " << g[15]<<"\n";
std::cout << "\n"<< "Route 3: "<<"\n";
for (int c= 0; c < 15; c++)
{
if( m[c] == 6 || m[c] == 8 )
std::cout<< "[r]-->"<<"\t";
else if (m[c] == 7 )
std::cout<< "[u]-->"<<"\t";
//else if (m[c] == 8)
// std::cout<<"[r]"<<"\t";
else
std::cout <<"";
}
std::cout << "\n";
}
void PrintRoute3( void )
{//std::cout << "Start Node : " << g[0] <<"\t"<< "Finish Node : " << g[15]<<"\n";
std::cout << "\n"<< "Route 4 : "<<"\n";
for (int c = 0; c <= 15; c++)
{
if( m[c] == 11)
std::cout<< "[u]-->"<<"\t";
else if (m[c] == 12)
std::cout<< "[r]-->"<<"\t";
else
std::cout <<"";
}
std::cout << "\n";
}
void PrintRoute4( void )
{//std::cout << "\n"<< "Start Node : " << g[0]<<"\t" << "Finish Node : " << g[15]<<"\n";
std::cout << "\n"<< "Route 2 : "<<"\n";
for (int c= 1; c < 16; c++)
{
if( m[c] == 13 )
std::cout<< "[u]-->"<<"\t";
else if (m[c] == 14 )
std::cout<< "[r]-->"<<"\t";
else if (m[c] == 15)
std::cout<<"[u]-->"<<"\t";
else
std::cout <<"";
}
}
int main( )
{ int E,E1,E2,E3;
int a, b;
int m[4][4];
ifstream in;
char state1[] = {'d','p','S','1','3'};
printmatrix(state1, 4);
cout<<"open test file : press";
in.open("C:/Users /test.txt");
if (!in) {
cout << "Cannot open file.\n";
system("pause");
return 0;
}
for (b = 1; b <= 4; b++) {cout << "\n";
for (a = 1; a <= 4; a++) {
in >> m[a][b];
cout <<"\t" << "["<<m[a][b] <<"]" ;
}
cout << "\n";
//
}
in.close();
std::cout << "\n";
int mt[6][6];
cout<<"open training file : press";
ifstream i;
i.open("C:/Users/training.txt");
if (!i) {
cout << "Cannot open file.\n";
return 0;
}
cout << "\n";
for (b = 1; b <= 6; b++) {cout << "\n";
for (a = 1; a <= 6; a++) {
i >> mt[a][b];
cout <<"\t" << "["<<mt[a][b] <<"]" ;
}
cout << "\n";
}
i.close();
std::cout << "\n";
system("pause");
int F = E = E1 = E2= E3;
std::cout<< "Enter Energy E";
//std::cin>>E >> E1 >> E2 >> E3 >>E4;
std::cin>>F;
std::cout << "Start Node : " << g[0] <<"\t"<< "Finish Node : " << g[15]<<"\n";
while( !Finished )
FindNextPos();
PrintRoute( );
nextpos1();
PrintRoute1();
nextpos2();
PrintRoute2();
nextpos3();
PrintRoute3();
nextpos4();
PrintRoute4();
std::cin.get( );
system("pause");
return 0;
}
You will need some kind of path finding algorithm.
The A* ("A-Star") algorithm is probably a good place to start. This page does a decent job of explaining it. If A* seems to complicated, you may want to try Dijkstra's algorithm first. Dijkstra's algorithm is the same as A*, but without the heuristic(estimated cost to goal).

Resources