#include "modelbahn.h"
#include "locomotive.h"


Locomotives::Locomotives () {
	pthread_mutex_init(&mtx, NULL);
	changed = 0;
	locomotives = (Locomotive*) malloc(sizeof(Locomotive)*LOCOMOTIVES_MAX);
	memset(locomotives, 0x0, sizeof(Locomotive)*LOCOMOTIVES_MAX);
	max = SENSORS_MAX;
};

Locomotives::~Locomotives() {
	free (locomotives);
	locomotives = NULL;
	max = 0;
};


int Locomotives::Lock() {
	if (pthread_mutex_lock(&mtx) == 0) return 1;
	else return 0;
}


int Locomotives::UnLock() {
	if (pthread_mutex_unlock(&mtx) == 0) return 1;
	else return 0;
}


JSONParse Locomotives::_GetJSON(int idx) {
	JSONParse json;
	JSONElement je;
	string s = "";

	json.Clear();

	s = locomotives[idx].name; json.AddObject("name", s);
	s = locomotives[idx].ifname; json.AddObject("ifname", s);
	s = locomotives[idx].blockdest; json.AddObject("blockdest", s);
	s = locomotives[idx].blocknext; json.AddObject("blocknext", s);
	s = locomotives[idx].blockassign; json.AddObject("blockassign", s);
	s = locomotives[idx].blockprev; json.AddObject("blockprev", s);
	s = locomotives[idx].auto_way; json.AddObject("auto_way", s);
	s = locomotives[idx].auto_wayold; json.AddObject("auto_wayold", s);
	s = locomotives[idx].schedway; json.AddObject("schedway", s);
	json.AddObject("addr", locomotives[idx].addr);
	json.AddObject("stepcode", locomotives[idx].stepcode);
	json.AddObject("speed", locomotives[idx].speed);
	json.AddObject("func", locomotives[idx].func);
	json.AddObject("flags", locomotives[idx].flags);
	json.AddObject("vmin", locomotives[idx].vmin);
	json.AddObject("vslow", locomotives[idx].vslow);
	json.AddObject("vmid", locomotives[idx].vmid);
	json.AddObject("vfast", locomotives[idx].vfast);
	json.AddObject("vmax", locomotives[idx].vmax);

	return json;
};


JSONParse Locomotives::GetJSON(string name) {
	int i;
	JSONParse jp;

	jp.Clear();

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (name.compare(locomotives[i].name) == 0) {
			jp = _GetJSON(i);
		}
	}

	UnLock();

	return jp;
};


void Locomotives::GetJSONAll(JSONParse *json) {
	int i, cnt;
	JSONElement je;

	Lock();

	//
	// write all railway data
	// create json object array manualy
	je.type = JSON_T_ARRAY;
	je.name = "locomotives";
	for (cnt = 0, i = 0; i < max; i++)
		if (locomotives[i].name[0] != 0) {
			if (cnt != 0) je.value += ",";			// not first element
			je.value += _GetJSON(i).ToString();
			cnt++;
		}
	json->AddObject(je);

	UnLock();
};


Locomotive Locomotives::GetLocomotiveFromJSON(JSONParse *j) {
	Locomotive l;
	string s;

	l.name[0] = 0;
	l.ifname[0] = 0;
	l.addr = 0;
	l.stepcode = 0;
	l.vmin = 0;
	l.vslow = 0;
	l.vmid = 0;
	l.vfast = 0;
	l.vmax = 0;
	l.flags = 0;
	l.speed = 0;
	l.func = 0;
	l.schedway[0] = 0;
	l.blockassign[0] = 0;
	l.blockdest[0] = 0;
	l.blocknext[0] = 0;
	l.blockprev[0] = 0;
	l.auto_way[0] = 0;
	l.auto_wayold[0] = 0;
	l.auto_onroute = 0;
	l.auto_data = 0;
	l.auto_timenext = {0};

	j->GetValue("name", &s);
	strncpy (l.name, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("ifname", &s);
	strncpy (l.ifname, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("blockassign", &s);
	strncpy (l.blockassign, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("blockdest", &s);
	strncpy (l.blockdest, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("blockprev", &s);
	strncpy (l.blockprev, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("blocknext", &s);
	strncpy (l.blocknext, s.c_str(), REFERENCENAME_LEN);
	j->GetValue("schedway", &s);
	strncpy (l.schedway, s.c_str(), WAYDATA_LEN);

	j->GetValueInt("addr", &l.addr);
	j->GetValueInt("stepcode", &l.stepcode);
	j->GetValueInt("speed", &l.speed);
	j->GetValueInt64("func", &l.func);
	j->GetValueInt("flags", &l.flags);
	j->GetValueInt("vmin", &l.vmin);
	j->GetValueInt("vslow", &l.vslow);
	j->GetValueInt("vmid", &l.vmid);
	j->GetValueInt("vfast", &l.vfast);
	j->GetValueInt("vmax", &l.vmax);

	return l;
};


/*
 * this function gets called if some configuration data has changed.
 * We will ignore the functions and speed settings. This has to be set with the SetFunction
 * and SetSpeed calls.
 */
int Locomotives::Change(Locomotive *loco) {
	int i;
	int ifree = -1;

	Lock();
	for (i = 0; i < max; i++) {
		if (locomotives[i].name[0] != 0) {
			// found element
			if (strncmp(locomotives[i].name, loco->name, REFERENCENAME_LEN) == 0) {
				// copy block data
				strncpy (locomotives[i].ifname, loco->name, REFERENCENAME_LEN);
				locomotives[i].addr = loco->addr;
				locomotives[i].stepcode = loco->stepcode;
				locomotives[i].vmin = loco->vmin;
				locomotives[i].vslow = loco->vslow;
				locomotives[i].vmid = loco->vmid;
				locomotives[i].vfast = loco->vfast;
				locomotives[i].vmax = loco->vmax;
				locomotives[i].flags = loco->flags;
				strncpy (locomotives[i].schedway, loco->schedway, WAYDATA_LEN);

				ifree = i;
				break;
			}
		}
		else if (ifree == -1) ifree = i;
	}

	// element not found add new element
	if (ifree != -1 && ifree < max) {
		locomotives[ifree] = *loco;
		strncpy (locomotives[ifree].name, loco->name, REFERENCENAME_LEN);
		strncpy (locomotives[ifree].ifname, loco->ifname, REFERENCENAME_LEN);
		strncpy (locomotives[ifree].schedway, loco->schedway, WAYDATA_LEN);
	}

	changed = 1;
	UnLock();

	return 1;
};


int Locomotives::Delete(string name) {
	int i;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (name.compare(locomotives[i].name) == 0) {
			locomotives[i].name[0] = 0;
			locomotives[i].ifname[0] = 0;
			locomotives[i].schedway[0] = 0;
			locomotives[i].addr = 0;
			locomotives[i].stepcode = 0;
			locomotives[i].vmin = 0;
			locomotives[i].vslow = 0;
			locomotives[i].vmid = 0;
			locomotives[i].vfast = 0;
			locomotives[i].vmax = 0;
			locomotives[i].flags = 0;
			locomotives[i].auto_way[0] = 0;
			locomotives[i].auto_wayold[0] = 0;
			locomotives[i].auto_onroute = -1;
			locomotives[i].auto_data = -1;
			locomotives[i].auto_timenext = {0};
			changed = 1;
			break;
		}
	}

	UnLock();

	return 1;
};


int Locomotives::SetSpeed(string name, int speed) {
	int i;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (name.compare(locomotives[i].name) == 0) {
			server->interfaces.SetLocoSpeed(&locomotives[i], speed);
			break;
		}
	}

	UnLock();

	return 1;
};


int Locomotives::SetReverse(string name, int reverse) {
	int i;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (name.compare(locomotives[i].name) == 0) {
			if (reverse)	locomotives[i].flags |= LOCO_F_REVERSE;
			else			locomotives[i].flags &= ~LOCO_F_REVERSE;

			server->interfaces.SetLocoSpeed(&locomotives[i], locomotives[i].speed);
			break;
		}
	}

	UnLock();

	return 1;
};


int Locomotives::SetFunction(string name, int func, int value) {
	int i;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (name.compare(locomotives[i].name) == 0) {
			server->interfaces.SetLocoFunction(&locomotives[i], func, value);
			break;
		}
	}
	UnLock();

	return 1;
};


int Locomotives::SetDestination (string name, string block, int direction) {
	int i;
	string way;
	int blflags;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {

			blflags = server->blocks.GetFlags(block);
			if ((blflags & BLOCK_F_SHORT) && !(locomotives[i].flags & LOCO_F_SHORTTRAIN) && !(blflags & BLOCK_F_SPLIT)) break;
			if ((blflags & BLOCK_F_ENDSTATION) && !(locomotives[i].flags & LOCO_F_CANREVERSE)) break;
			if ((blflags & BLOCK_F_ONLYCARGO) && !(locomotives[i].flags & LOCO_F_CARGO)) break;
			if ((blflags & BLOCK_F_ONLYPASSENGER) && (locomotives[i].flags & LOCO_F_CARGO)) break;

			if (direction)	snprintf (locomotives[i].blockdest, REFERENCENAME_LEN, "-:%s", block.c_str());
			else			snprintf (locomotives[i].blockdest, REFERENCENAME_LEN, "+:%s", block.c_str());

			//
			// check if it is a split block and a long train
			if (!(locomotives[i].flags & LOCO_F_SHORTTRAIN) && (blflags & BLOCK_F_SPLIT)) {
				if ((locomotives[i].blockdest[0] == '+' && (blflags & BLOCK_F_SPLITPOS)) ||
					(locomotives[i].blockdest[0] == '-' && !(blflags & BLOCK_F_SPLITPOS))	) {
					strncpy (locomotives[i].blockdest+2
							, server->blocks.GetSecondBlock(locomotives[i].blockdest+2).c_str()
							, REFERENCENAME_LEN-2);
				}
			}

			debug (0, "Locomotives::SetDestination (Name:%s Block:%s Direction:%d) -> Final:%s"
					, name.c_str(), block.c_str(), direction, locomotives[i].blockdest);

			break;
		}
	UnLock();

	return 1;
}


int Locomotives::SetAssign (string name, string block, int direction) {
	int i, x = -1;
	int y = -1;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {
			debug (0, "Locomotives::SetAssign (Name:%s Block:%s Direction:%d)", name.c_str(), block.c_str(), direction);

			if (locomotives[i].flags & LOCO_F_AUTO) {
				debug (0, "Locomotives::SetAssign not possible Loco is stil in AUTO mode.");
				i = max;
				continue;
			}

			if (locomotives[i].blockassign[0] != 0) // still locked unlock
				server->blocks.SetLockedby(locomotives[i].blockassign+2, name, 0);

			if (direction)	snprintf (locomotives[i].blockassign, REFERENCENAME_LEN, "-:%s", block.c_str());
			else			snprintf (locomotives[i].blockassign, REFERENCENAME_LEN, "+:%s", block.c_str());

			server->blocks.SetLockedby(block, name, 1);

			break;
		}
	UnLock();

	if (i < max && i >=0) {
		int cnt = 0;
		while (server->railways.FindReference(&x, &y, block, (cnt++))) {
			string bl = server->blocks.GetLockedby(block);
			if(bl.length() == 0 || bl.compare(name) == 0) {
				server->blocks.SetLockedby(block, name, 1);
				server->railways.SetLockedby(x, y, name, 1);
			}
		}
	}

	printf ("%s:%d %s ************** finish me **************\n", __FILE__, __LINE__, __FUNCTION__);
	printf ("%s:%d %s * clear old refferences from loco      \n", __FILE__, __LINE__, __FUNCTION__);
	printf ("%s:%d %s * stop loco                            \n", __FILE__, __LINE__, __FUNCTION__);
	return 1;
}


int Locomotives::GetFlags(string name) {
	int flags = -1;
	int i;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {
			flags = locomotives[i].flags;
		}
	UnLock();

	return flags;
}


int Locomotives::Reset(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {
			debug (0, "Locomotives::Reset (Name:%s)", name.c_str());
			locomotives[i].flags &= ~(LOCO_F_AUTO | LOCO_F_AUTOSTOP | LOCO_F_AUTORANDOM | LOCO_F_AUTOSHED);
			locomotives[i].blockassign[0] = 0;
			locomotives[i].blockdest[0] = 0;
			locomotives[i].blockprev[0] = 0;
			locomotives[i].blocknext[0] = 0;
			locomotives[i].auto_way[0] = 0;
			locomotives[i].auto_wayold[0] = 0;
			locomotives[i].auto_onroute = 0;
			locomotives[i].auto_data = 0;
			locomotives[i].auto_timenext = { 0 };
			locomotives[i].sched_step = -1;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}

	server->railways.ClearLockedby(name);
	server->blocks.ClearLockedby(name);

	UnLock();
	return 1;
};

int Locomotives::SetModeMan(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {
			debug (0, "Locomotives::SetMan (Name:%s)\n", name.c_str());
			locomotives[i].flags &= ~(LOCO_F_AUTO | LOCO_F_AUTOSTOP | LOCO_F_AUTORANDOM | LOCO_F_AUTOSHED);
			locomotives[i].auto_onroute = 0;
			locomotives[i].auto_way[0] = 0;
			locomotives[i].auto_wayold[0] = 0;
			locomotives[i].sched_step = -1;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}
	UnLock();
	SetSpeed(name, 0);

	return 1;
};

int Locomotives::SetModeAutoMan(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0 && (locomotives[i].flags & LOCO_F_AUTO)) {
			debug (0, "Locomotives::SetAutoMan (Name:%s)\n", name.c_str());
			locomotives[i].flags |= LOCO_F_AUTOSTOP;
			locomotives[i].flags &= ~LOCO_F_AUTORANDOM;
			locomotives[i].flags &= ~LOCO_F_AUTOSHED;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}
	UnLock();

	return 1;
};

int Locomotives::SetModeAuto(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0) {
			debug (0, "Locomotives::SetAuto (Name:%s)\n", name.c_str());
			locomotives[i].flags |= LOCO_F_AUTO;
			locomotives[i].flags &= ~LOCO_F_AUTORANDOM;
			locomotives[i].flags &= ~LOCO_F_AUTOSHED;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}
	UnLock();

	return 1;
};

int Locomotives::SetModeAutoRand(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0 && (locomotives[i].flags & LOCO_F_AUTO)) {
			debug (0, "Locomotives::SetAutoRandom (Name:%s)\n", name.c_str());
			locomotives[i].flags |= LOCO_F_AUTORANDOM;
			locomotives[i].flags &= ~LOCO_F_AUTOSHED;
			locomotives[i].flags &= ~LOCO_F_AUTOSTOP;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}
	UnLock();

	return 1;
};


int Locomotives::SetModeAutoShed(string name) {
	int i;
	JSONParse jp;

	Lock();
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0)
		if (name.compare(locomotives[i].name) == 0 && (locomotives[i].flags & LOCO_F_AUTO)) {
			debug (0, "Locomotives::SetAutoShed (Name:%s)\n", name.c_str());
			locomotives[i].flags |= LOCO_F_AUTOSHED;
			locomotives[i].flags &= ~LOCO_F_AUTORANDOM;
			locomotives[i].flags &= ~LOCO_F_AUTOSTOP;

			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			break;
		}
	UnLock();

	return 1;
};


//
// set values from bus...
//
int Locomotives::SetSpeedFromBus(string ifname, int addr, int speed) {
	int i;
	JSONParse jp;


	debug (0, "Locomotives::SetSpeedFromBus IfName:%s Addr:%d Speed:%d", __FILE__, __LINE__, ifname.c_str(), addr, speed);

	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (ifname.compare(locomotives[i].ifname) == 0 && locomotives[i].addr == addr) {
			int maxstep;
			switch(locomotives[i].stepcode) {
				case LOCO_INT_DCC14: maxstep = 14; break;
				case LOCO_INT_DCC28: maxstep = 28; break;
				case LOCO_INT_DCC128: maxstep = 126; break;
				default: maxstep = -1; break;
			}

			if (speed == 0) locomotives[i].speed = 0;
			if (speed >= (127-((127/maxstep)+1))) locomotives[i].speed = locomotives[i].vmax;
			else {
				locomotives[i].speed = speed * locomotives[i].vmax / 127;
				if (locomotives[i].speed > locomotives[i].vmax) locomotives[i].speed = locomotives[i].vmax;
			}

			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			return 1;
		}
	}
	return 0;
};


int Locomotives::SetFunctionFromBus(string ifname, int addr, int func) {
	int i;
	JSONParse jp;

	debug (0, "Locomotives::SetDirectionFromBus IfName:%s Addr:%d function:%d", __FILE__, __LINE__, ifname.c_str(), addr, func);
	for (i = 0; i < max; i++) if (locomotives[i].name[0] != 0) {
		if (ifname.compare(locomotives[i].ifname) == 0 && locomotives[i].addr == addr) {
			if (func & 32)	locomotives[i].flags |= LOCO_F_REVERSE;
			else			locomotives[i].flags &= ~LOCO_F_REVERSE;

			jp.AddObject("locomotive",_GetJSON(i));
			if(network) network->ChangeListPushToAll(jp.ToString());

			return 1;
		}
	}
	return 0;
};


string Locomotives::GetName(int idx) {
	string result = "";

	Lock();
	if (idx <= max && idx >= 0)
		result = locomotives[idx].name;
	UnLock();

	return result;
};


/*
 * Locomotive is on auto, do the scheduled steps
 */
int Locomotives::SchedulerNextStep(Locomotive *loc) {
	int stepsmax = 0;		// number of found steps
	int stepcurpos = 0;		// current pos in way string
	int stepnextpos = -1;	// next pos;
	int schedwaylen = 0;
	int i;
	string name;
	string next;

	if (loc == NULL) return 0;

	debug (0, "Locomotives::SchedulerNextStep  loc:%s   Step:%d", loc->name, loc->sched_step);
	name = loc->name;

	//
	// find number of scheduled steps, current pos and next pos
	for (i = 0; i < WAYDATA_LEN && loc->schedway[i] != 0; i++)
		if (loc->schedway[i] == ',') {
			stepsmax++;
			if (stepsmax == loc->sched_step) stepcurpos = i;
			else if (loc->sched_step == 0 || (stepcurpos > 0 && stepnextpos == -1)) stepnextpos = i+1;
	}
	schedwaylen = i;
	if (stepnextpos == -1) stepnextpos = 0;	// no next step found point to first

	//
	// set next way
	if (stepnextpos == 0) loc->sched_step = 0;
	else loc->sched_step++;
	if (loc->sched_step > stepsmax) {
		loc->sched_step = 0;
		stepnextpos = 0;
	}

	// read parameter of step
	for (next = "", i = stepnextpos; i < WAYDATA_LEN && loc->schedway[i] != ',' && loc->schedway[i] != 0; i++) {
		if (i > stepnextpos+1) next += loc->schedway[i];
	}
	debug (0, "Locomotives::SchedulerNextStep  loc:%s Next Pos: '%s'", loc->name, loc->schedway+stepnextpos);

	if (loc->schedway[stepnextpos] && stepnextpos < schedwaylen-2) {

		switch (loc->schedway[stepnextpos]) {
		case ('W'):
		case ('w'):
			gettimeofday (&loc->auto_timenext, NULL);
			loc->auto_timenext.tv_sec += atoi (next.c_str());
			loc->auto_data = 0;
			loc->auto_onroute = LOCO_OR_STOPWAIT;
			break;
		case ('B'):
		case ('b'):
			gettimeofday (&loc->auto_timenext, NULL);
			snprintf (loc->blockdest, REFERENCENAME_LEN, "%s", next.c_str());
			loc->auto_timenext.tv_sec += atoi (next.c_str());
			loc->auto_data = 0;
			loc->auto_onroute = LOCO_OR_SEARCH;
			break;
		case ('R'):
		case ('r'):
			if (loc->blockassign[0] == '-') loc->blockassign[0] = '+';
			else if (loc->blockassign[0] == '+') loc->blockassign[0] = '-';

			if (loc->flags & LOCO_F_REVERSE)	loc->flags &= ~LOCO_F_REVERSE;
			else								loc->flags |= LOCO_F_REVERSE;

			break;
		default:
			debug (0, "Locomotives::SchedulerNextStep  Unknown Command: '%c' loc:%s  way:'%s'", loc->name,
							loc->schedway[stepnextpos], loc->schedway);
			break;
		}
	}

	SendUpdate(loc);

	return 0;
}



//
// with each call only check one single step of the way, if we have to
// turn a turnout try to do it and return 0.
// if everything is set up till the destination (next) block return 1
// to speed things up: we only prepare ways which we have locked already
//
int Locomotives::AutoCheckWaySingleStep(string way, Locomotive *loc, int *data) {
	size_t pos1;
	size_t curpos;
	int cnt;
	int state;
	int newdata = 0;
	string turnout;
	string nextblock;
	size_t nextblockpos;
	int blflags = 0;
	int x, y;
	Railway r;

	if (*data < 0) *data = 0;
	debug (0, "Locomotives::AutoCheckWaySingleStep Prepare for Loco: %s Way:%s  data:%d blocknext:%s", loc->name, way.c_str(), *data, loc->blocknext);

	//
	// find final next block
	// check if it is a split block and a long train
	blflags = server->blocks.GetFlags(loc->blocknext+2);
	nextblock = loc->blocknext+2;
	if (!(loc->flags & LOCO_F_SHORTTRAIN) && (blflags & BLOCK_F_SPLIT)) {
		if ((loc->blocknext[0] == '+' && (blflags & BLOCK_F_SPLITPOS)) ||
			(loc->blocknext[0] == '-' && !(blflags & BLOCK_F_SPLITPOS))	) {
			nextblock = server->blocks.GetSecondBlock(loc->blocknext+2);
		}
	}
	nextblockpos = way.find(nextblock);
	debug (0, "Locomotives::AutoCheckWaySingleStep nextblock:%s", nextblock.c_str());

	curpos = 0;
	do {
		//
		// read all ways from the begin with stop at "data"
		curpos = way.find (",t:", curpos+1);
		if (curpos != string::npos && nextblockpos != string::npos && curpos > nextblockpos) break;

		newdata++;
		if (curpos != string::npos) {
			pos1 = way.find(":", curpos+3);
			if (pos1 == string::npos) {
				debug (0, "%s:%d turnout without value?  '%s'", __FILE__, __LINE__, way.c_str());
				server->PowerOnOff(0);
				server->SetModeManual();
				return 0;
			}
			turnout = way.substr(curpos+3, pos1-curpos-3);

			state = server->turnouts.Get(turnout);
			if (state == -1) {
				debug (0, "%s:%d turnout not found '%s'", __FILE__, __LINE__, turnout.c_str());
				server->PowerOnOff(0);
				server->SetModeManual();
				return 0;
			}

			cnt = 0;
			while (server->railways.FindReference(&x, &y, turnout, cnt++)) {
				cnt++;
				r = server->railways.RailwayGet(x, y);
				debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s LockedBy:%s", loc->name, turnout.c_str(), r.lockedby);

				//
				//
				if (strncmp(loc->name, r.lockedby, REFERENCENAME_LEN) == 0 || r.lockedby[0] == 0) {
					if (way[pos1+1] == '0' && state != 0) {
						debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Not Equal -> Set:0", loc->name, turnout.c_str());
						server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
						return 0;
					}
					else if (way[pos1+1] == '1' && state != 1) {
						debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Not Equal -> Set:1", loc->name, turnout.c_str());
						server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
						return 0;
					}
					else if (newdata > *data) {
						debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Equal Reset:%c", loc->name, turnout.c_str(), way[pos1+1]);
						if (way[pos1+1] == '0') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
						if (way[pos1+1] == '1') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
						(*data) = newdata;
						return 0;
					}
				}
			}
			if (cnt == 0) {
				debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Reference not found", loc->name, turnout.c_str());
				return 0;
			}
		}
	} while (curpos != string::npos);

	if (curpos == string::npos || (nextblockpos != string::npos && curpos > nextblockpos)) return 1;
	return 0;
}


void::Locomotives::SendUpdate (Locomotive *loc) {
	int lnum;
	JSONParse jp;

	if (locomotives == NULL) return;

	for (lnum = 0; lnum < max; lnum++) {
		if (loc == &locomotives[lnum]) {
			jp.Clear();
			jp.AddObject("locomotive",_GetJSON(lnum));
			if(network) network->ChangeListPushToAll(jp.ToString());
			break;
		}
	}
}


/*
 * locomotive searching for a new way
 * 		loco->way and loco->blocknext will be overwritten
 * 		loco->way must be saved to loco->wayold before calling this function
 * 		it will not set the auto_onroute step
 */
int Locomotives::Loco_SearchAndLock(Locomotive *loco) {
	string way;

	//
	// try to find and prepare(lock) a new way.
	// nothing found check if we can reverse direction, this will be done only
	// once a second (LOCO_TO_TRYAGAIN)
	//
	// destination empty? true  -> random
	//                    false -> find way to destination
	debug (0, "Locomotives::Loop Search  '%s'  Reverse:%d", loco->name, (loco->flags & LOCO_F_REVERSE) ? 1 : 0);

	//
	// destination set? - need to find a random block?
	if (loco->blockdest[0] == 0) {
		if (loco->flags & LOCO_F_AUTORANDOM) {
			if (server->railways.FindRandomWay(loco->blockassign, loco->name, &way)) {
				size_t pos, pos1;
				if ((pos = way.find(",b:", 1)) != string::npos) {
					if ((pos1 = way.find(",", pos+3)) != string::npos) {
						strncpy (loco->blocknext, way.substr(pos+3, pos1-(pos+3)).c_str(), REFERENCENAME_LEN);
					}
					else strncpy (loco->blocknext, way.substr(pos+3,string::npos).c_str(), REFERENCENAME_LEN);
					strncpy (loco->auto_way, way.c_str(), WAYDATA_LEN);
					if (server->railways.LockWay(way, loco->name) == 1) {
						loco->auto_data = -1;
						return 1;
					}
					server->railways.UnLockWay(way,  loco->name);
				}
			}
		}
		else if (loco->flags & LOCO_F_AUTOSHED) {
			SchedulerNextStep(loco);
		}
	}

	//
	// we assume a destination is set, and try to find a new way to the destination
	else if (server->railways.FindWay(loco->blockassign, loco->blockdest, loco->name, &way)) {
		// try to lock way.
		size_t pos, pos1;
		debug (0, "Locomotives::Loop %s:%d Found Way:%s", __FILE__, __LINE__, way.c_str());
		if ((pos = way.find(",b:", 1)) != string::npos) {
			if ((pos1 = way.find(",", pos+3)) != string::npos) {
				strncpy (loco->blocknext, way.substr(pos+3, pos1-(pos+3)).c_str(), REFERENCENAME_LEN);
			}
			else strncpy (loco->blocknext, way.substr(pos+3,string::npos).c_str(), REFERENCENAME_LEN);
			strncpy (loco->auto_way, way.c_str(), WAYDATA_LEN);
			if (server->railways.LockWay(way, loco->name) == 1) {
				loco->auto_data = -1;
				return 1;
			}
			server->railways.UnLockWay(way,  loco->name);
		}
	}

	return 0;
}


/*
 * prepare to lock the way set up in way
 * the way is set and locked already, The turnouts will be set one by one with a time delay
 * between the commands. All turnouts will get a SetWay command in case some turnout was set
 * manualy without feedback to the server.
 */
int Locomotives::Loco_PrepareWay(Locomotive *loco) {
	string block;

	block = ((string)(loco->blocknext+2)).substr(0, ((string)(loco->blocknext+2)).find(","));

	if (loco->auto_timenext.tv_sec == 0 || timer_get(&loco->auto_timenext) > LOCO_TO_TURNOUT) {
		if (AutoCheckWaySingleStep(loco->auto_way, loco, &loco->auto_data) == 1)	{
			if ((loco->flags & LOCO_F_CARGO) ||
				(server->blocks.GetFlags(block) & BLOCK_F_SPEEDLIMIT)) return 1;
			else return 2;
		}
		timer_start(&loco->auto_timenext);
	}

	return 0;
}


/*
 * loco is on route, check if entered a block
 * returns if we entered the block, set the locos speed
 * search for next way and set auto_onroute to LOCO_OR_ENTERBLOCKNEXT or ENTERBLOCKSTOP
 * copy way to wayold. If a new way is found set the way if not leave empty
 */
int Locomotives::Loco_OnRoute(Locomotive *loco) {
	string block;
	string s_enter;
	string s_slow;
	string s_stop;
	int dir_reverse = 0;
	char *tmpc = NULL;
	string way;
	int blockflags;

	// FIXME: move this code to the block class -> we will need a better code for multiple sensors
	//
	// the train is on the way, check for entering the block.
	//
	block = ((string)(loco->blocknext+2)).substr(0, ((string)(loco->blocknext+2)).find(","));

	if (loco->blocknext[0] == '-')	dir_reverse = 1;
	else							dir_reverse = 0;

	s_stop = server->blocks.GetSensorStop(dir_reverse, block, loco->flags);
	s_slow = server->blocks.GetSensorSlow(dir_reverse, block, loco->flags);
	s_enter = server->blocks.GetSensorEnter(dir_reverse, block, loco->flags);

	if (    server->sensors.GetActive(s_enter) == 1 || server->sensors.GetActive(s_stop) == 1 ||
		    server->sensors.GetActive(s_slow) == 1) {   // entering block?

		debug (0, "* Locomotive '%s' EnterBlock '%s'", loco->name, loco->blocknext);
		debug (0, "* %s,%d      Sensor      Enter   '%s' = %d", __FILE__, __LINE__, s_enter.c_str(), server->sensors.GetActive(s_enter));
		debug (0, "* %s,%d      Sensor      Slow    '%s' = %d", __FILE__, __LINE__, s_slow.c_str(), server->sensors.GetActive(s_slow));
		debug (0, "* %s,%d      Sensor      Stop    '%s' = %d", __FILE__, __LINE__, s_stop.c_str(), server->sensors.GetActive(s_stop));

		// assignment <-- next, the assignment has to be checked
		// against long trains and split blocks
		// check if we need to clear dest

		strncpy (loco->blockprev, loco->blockassign, REFERENCENAME_LEN);
		tmpc = strstr (loco->blocknext, ",b:");
		if (tmpc)	strncpy (loco->blockassign, tmpc+3, REFERENCENAME_LEN);
		else		strncpy (loco->blockassign, loco->blocknext, REFERENCENAME_LEN);

		//
		// check if it is a split block and a long train
		blockflags = server->blocks.GetFlags(loco->blockassign+2);
		if (!(loco->flags & LOCO_F_SHORTTRAIN) && (blockflags & BLOCK_F_SPLIT)) {
			if ((loco->blockassign[0] == '+' && (blockflags & BLOCK_F_SPLITPOS)) ||
				(loco->blockassign[0] == '-' && !(blockflags & BLOCK_F_SPLITPOS))	) {
				strncpy (loco->blockassign+2
						, server->blocks.GetSecondBlock(loco->blockassign+2).c_str()
						, REFERENCENAME_LEN-2);
			}
		}

		strncpy (loco->auto_wayold, loco->auto_way, WAYDATA_LEN);
		loco->auto_way[0] = 0;
		loco->blocknext[0] = 0;

		debug (0, "Locomotive::Loco_OnRoute Loco:'%s' Prev:'%s' Assign:'%s' Next:'%s'",
				loco->name, loco->blockprev, loco->blockassign, loco->blocknext);

		//
		if (Loco_SearchAndLock(loco) == 1) {
			loco->auto_onroute = LOCO_OR_ENTERBLOCKNEXT;
			debug (0, "Locomotives::Loco_OnRoute Found Way Prepare '%s'\n", loco->name);
			SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vslow : loco->vslow);
			timer_start(&loco->auto_timenext);
		}
		else {
			loco->auto_onroute = LOCO_OR_ENTERBLOCKSTOP;
			debug (0, "Locomotives::Loco_OnRoute Slow Down '%s'\n", loco->name);
			SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vslow : loco->vslow);
		}
		return 1;

	}
	return 0;
}



/*
 * loco entered a block, check if need to stop
 */
int Locomotives::Loco_BlockEnterStop(Locomotive *loco) {
	string block;
	string s_enter;
	string s_slow;
	string s_stop;
	int dir_reverse = 0;
	string way;

	//
	// check if the train is in its stop position
	//
	block = ((string)(loco->blocknext+2)).substr(0, ((string)(loco->blocknext+2)).find(","));

	if (loco->blockassign[0] == '-')	dir_reverse = 1;
	else								dir_reverse = 0;

	s_stop = server->blocks.GetSensorStop(dir_reverse, block, loco->flags);
	s_slow = server->blocks.GetSensorSlow(dir_reverse, block, loco->flags);
	s_enter = server->blocks.GetSensorEnter(dir_reverse, block, loco->flags);
	if (server->sensors.GetActive(s_stop) == 1) {
		debug (0, "Locomotives::Loop BlockEnterStop '%s' UnLockWay\n", loco->name);
		debug (0, "* %s,%d      Sensor      Enter   '%s' = %d", __FILE__, __LINE__, s_enter.c_str(), server->sensors.GetActive(s_enter));
		debug (0, "* %s,%d      Sensor      Slow    '%s' = %d", __FILE__, __LINE__, s_slow.c_str(), server->sensors.GetActive(s_slow));
		debug (0, "* %s,%d      Sensor      Stop    '%s' = %d", __FILE__, __LINE__, s_stop.c_str(), server->sensors.GetActive(s_stop));

		SetSpeed(loco->name, 0);
		loco->auto_onroute = LOCO_OR_STOPWAIT;
		server->railways.UnLockWay(loco->auto_wayold, loco->name);
		server->blocks.SetLockedby(loco->blockprev+2, loco->name, 0);
		loco->auto_wayold[0] = 0;
		timer_start(&loco->auto_timenext);
	}

	return 0;
}


/*
 * loco entered a block, check if need to stop
 */
int Locomotives::Loco_BlockEnterNext(Locomotive *loco) {
	int dir_reverse;
	string block;
	string s_enter;
	string s_slow;
	string s_stop;
	string s_shortstop;
	string way;

	//
	// check if the train is in its stop position
	//
	block = ((string)(loco->blockassign+2)).substr(0, ((string)(loco->blockassign+2)).find(","));

	if (loco->blockassign[0] == '-')	dir_reverse = 1;
	else								dir_reverse = 0;

	s_stop = server->blocks.GetSensorStop(dir_reverse, block, loco->flags);
	s_slow = server->blocks.GetSensorSlow(dir_reverse, block, loco->flags);
	s_enter = server->blocks.GetSensorEnter(dir_reverse, block, loco->flags);

	if (server->sensors.GetActive(s_stop) == 1) {
		if (!AutoCheckWaySingleStep(loco->auto_way, loco, &loco->auto_data)) {
			//
			// end of block and not finished preparing the way.
			// stop everything
			debug (0, "Locomotives::Loco_BlockEnterNext could not prepare way in time '%s'\n", loco->name);
			SetSpeed(loco->name, 0);
			loco->auto_onroute = LOCO_OR_STOPWAIT;
			server->railways.UnLockWay(loco->auto_wayold, loco->name);
			server->blocks.SetLockedby(loco->blockprev+2, loco->name, 0);
			loco->auto_wayold[0] = 0;
			timer_start(&loco->auto_timenext);

			return 0;
		}
		else {
			debug (0, "* Locomotives::Loco_BlockEnterNext endblock reached %s", loco->name);

			server->railways.UnLockWay(loco->auto_wayold, loco->name);
			server->blocks.SetLockedby(loco->blockprev+2, loco->name, 0);
			loco->auto_wayold[0] = 0;

			if ((loco->flags & LOCO_F_CARGO) ||
				(server->blocks.GetFlags(block) & BLOCK_F_SPEEDLIMIT)) return 1;
			else return 2;
		}
	}

	if (loco->auto_timenext.tv_sec > 0 && timer_get(&loco->auto_timenext) > LOCO_TO_TURNOUT) {
			debug (0, "* Locomotives::Loco_BlockEnterNext prepare way for loco: %s", loco->name);
			if (AutoCheckWaySingleStep(loco->auto_way, loco, &loco->auto_data) == 1)	{
				//
				// finished preparing new way, need to stay in ENTERBLOCKNEXT to unlock old way
				debug (0, "* Locomotives::Loco_BlockEnterNext finished preparing way for loco: %s", loco->name);
				loco->auto_timenext.tv_sec = 0;
			}
			else timer_start(&loco->auto_timenext);
	}

	return 0;
}


/*
 * loco stopped on block, wait some time and continue with the next destination
 */
int Locomotives::Loco_BlockStopWait(Locomotive *loco) {
	if (loco->auto_timenext.tv_sec == 0 || timer_get(&loco->auto_timenext) > 5000) {
		if (loco->blockdest[0] != 0)	loco->auto_onroute = LOCO_OR_SEARCH;
		else							loco->auto_onroute = LOCO_OR_NOTHING;
	}
	return 1;
}



int Locomotives::Loop() {
	int lnum;
	string way;
	Locomotive *loco = NULL;
	JSONParse jp;

	for (lnum = 0; lnum < max; lnum++) if (locomotives[lnum].name[0] != 0) {
		loco = &locomotives[lnum];

		if (loco->flags & LOCO_F_AUTO) {
			//
			// only in automate do anything alone
			//
//			debug (0, "* Locomotives::Loop (%s:%d) Loco:%s auto_onroute:%d assign:'%s' next:'%s' prev:'%s' dest:'%s' way:'%s' wayold:'%s'"
//					, __FILE__, __LINE__, loco->name, loco->auto_onroute, loco->blockassign
//					, loco->blocknext, loco->blockprev, loco->blockdest, loco->auto_way, loco->auto_wayold);

			if (loco->blockassign[0] == 0) {
				debug (0, "%s:%d Locomotive [%s] not assigned to any block. Set Mode to Man", __FILE__, __LINE__, loco->name);
				SetModeMan(loco->name);
				continue;
			}

			if (loco->auto_onroute == LOCO_OR_NOTHING) {
				// loco mode is set to autostop -> set loco to man
				if (loco->flags & LOCO_F_AUTOSTOP) {
					SetModeMan(loco->name);
					continue;
				}
				else {
					debug (0, "%s:%d Locomotive [%s] is doing NOTHING", __FILE__, __LINE__, loco->name);
					timer_start(&loco->auto_timenext);
					loco->auto_onroute = LOCO_OR_SEARCH;
				}
			}

			//
			//
			else if (loco->auto_onroute == LOCO_OR_SEARCH) {
				// loco mode is set to autostop -> set loco to man
				if (loco->flags & LOCO_F_AUTOSTOP) {
					SetModeMan(loco->name);
					continue;
				}

				if (loco->auto_timenext.tv_sec == 0 || timer_get(&loco->auto_timenext) > LOCO_TO_TRYAGAIN) {
					timer_start(&loco->auto_timenext);

					if (strcmp(loco->blockassign, loco->blockdest) == 0) {
						loco->blockdest[0] = 0;
						debug (0, "* Locomotive '%s' DEST == ASSING", loco->name);
						jp.Clear();
						jp.AddObject("locomotive",_GetJSON(lnum));
						if(network) network->ChangeListPushToAll(jp.ToString());
					}

					// try to find and lock a way
					if (Loco_SearchAndLock(loco)) {
						loco->auto_onroute = LOCO_OR_PREPARE;
					}
					else {
						// nothing found -> try reverse
						if (loco->blockassign[0] != 0 && loco->blockdest[0] != 0 && (loco->flags & LOCO_F_CANREVERSE)) {
							debug (0, "* Loco_SearchAndLock Reverse Loco %s", loco->name);
							if (loco->blockassign[0] == '-') loco->blockassign[0] = '+';
							else if (loco->blockassign[0] == '+') loco->blockassign[0] = '-';

							if (loco->flags & LOCO_F_REVERSE)	loco->flags &= ~LOCO_F_REVERSE;
							else								loco->flags |= LOCO_F_REVERSE;

							jp.Clear();
							jp.AddObject("locomotive",_GetJSON(lnum));
							if(network) network->ChangeListPushToAll(jp.ToString());
						}
					}
				}
			}

			//
			//
			else if (loco->auto_onroute == LOCO_OR_PREPARE) {
				switch (Loco_PrepareWay(loco)) {
				case 1:
					SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vmid : loco->vmid);
					loco->auto_onroute = LOCO_OR_ONTHEWAY;
					debug (0, "* %s:%d Locomotive '%s' Way Prepared -> Speed: VMID", __FILE__, __LINE__, loco->name);
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
					break;
				case 2:
					SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vfast : loco->vfast);
					loco->auto_onroute = LOCO_OR_ONTHEWAY;
					debug (0, "* %s:%d Locomotive '%s' Way Prepared -> Speed: VFAST", __FILE__, __LINE__, loco->name);
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
					break;
				default:
					break;
				}
			}

			else if (loco->auto_onroute == LOCO_OR_ONTHEWAY) {
				if (Loco_OnRoute(loco)) {
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
				}

			}

			else if (loco->auto_onroute == LOCO_OR_ENTERBLOCKSTOP) {
				if (Loco_BlockEnterStop(loco)) {
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
				}
			}

			else if (loco->auto_onroute == LOCO_OR_ENTERBLOCKNEXT) {
				switch (Loco_BlockEnterNext(loco)) {
				case 1:
					SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vmid : loco->vmid);
					loco->auto_onroute = LOCO_OR_ONTHEWAY;
					debug (0, "* %s:%d Locomotive LOCO_OR_ENTERBLOCKNEXT '%s' Way Prepared -> Speed: VMID", __FILE__, __LINE__, loco->name);
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
					break;
				case 2:
					SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vfast : loco->vfast);
					loco->auto_onroute = LOCO_OR_ONTHEWAY;
					debug (0, "* Locomotive LOCO_OR_ENTERBLOCKNEXT '%s' Way Prepared -> Speed: VFAST", loco->name);
					jp.Clear();
					jp.AddObject("locomotive",_GetJSON(lnum));
					if(network) network->ChangeListPushToAll(jp.ToString());
					break;
				default:
					break;
				}
			}

			else if (loco->auto_onroute == LOCO_OR_STOPWAIT) {
				Loco_BlockStopWait(loco);
			}
		}
	}

	return 0;
}