//#include <util/MThread.h>
#include "thread.h"
#include "ui_mainwindow.h"
#include <util/MThread.h>

#define DEG_TO_RAD(d)	((d)*M_PI/180.0)
#define RAD_TO_DEG(r)	((r)*180.0/M_PI)

Thread::Thread(AmtecManipulator* pManipulator, QTableWidget* tableW)
{
	mManipulator = pManipulator;
	tableWidget = tableW;
}

void Thread::run() {
		while (1) {
			mManipulator->setSyncMotionMode(true);
				tMotion = AmtecManipulator::MOTION_FRAMP_MODE;
				double position[MODULE_SIZE];
				int row = 0;
				while ((tableWidget->item(row, 1)) != 0) {
					int column = 1;
					for (int count = 0; count < MODULE_SIZE; count++) {
						position[count]
								= tableWidget->item(row, column++)->text().toDouble();

						if (count == MODULE_SIZE - 1) { // für den Greifer
							mManipulator->execMotion(count + 20, tMotion, position[count]
									/ 1000, 2);
						} else {
							mManipulator->execMotion(count + 20, tMotion,
									DEG_TO_RAD(position[count]), 2);
						}
					}

					mManipulator->execSyncMotion();

					//Überprüfung, ob der Module in Bewegung sind

					uint8_t count;
					for (count = 0; count < MODULE_SIZE; count++) {
						const AmtecManipulator::ModuleVector& tModules =
								mManipulator->getModules();
						sleep(1);
						const AmtecManipulator::ModuleConfig& tModule = tModules[count];
						while (tModule.status_flags.flags.motion == 1) {
							const AmtecManipulator::ModuleVector& tModules_act =
									mManipulator->getModules();
							sleep(1);
							const AmtecManipulator::ModuleConfig& tModule_act =
									tModules_act[count];
							if (tModule_act.status_flags.flags.motion != 1) {
								break;
							}
							usleep(10000);
						}
					}
					row++;
				}
		}
}