#include <stdio.h>
#include <stdint.h>
#include "libschsat.h"
#define LSS_OK 0 
#define LSS_ERROR 1 
#define LSS_BREAK 2
#include <math.h> 
#include <time.h>

/*Коэффициент дифференциальной обратной связи.
 Коэффициент положительный, если маховик расположен осью z вверх
 и ДУС расположен осью z также вверх.
 Коэффициент подбирается экспериментально в зависимости от формы
 и массы вашего спутника.*/
const float kd = 200.0;

// Временной шаг работы алгоритма, с
const float time_step = 0.1;

/* Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0.*/
const float omega_goal = 0.0;
// Максимально допустимая скорость маховика, об/мин
const int mtr_max_speed = 5000;
const uint16_t mtr_num = 1;			// Номер маховика
const uint16_t hyr_num = 1; 		        // Номер ДУС
const uint16_t mag_num = 1;			// Номер магнитометра
// Номер результата измерений
int i = 1;
// Угол поворота текущий
const float alpha = 0.0;


 
void initialize_all(void){/* Функция включает все приборы, 
которые будут использоваться в основной программе.*/
	printf("Enable angular velocity sensor №%d\n", hyr_num); 
	hyro_turn_on(hyr_num);
	Sleep(1);
	printf("Enable magnetometer %d\n", mag_num);
	magnetometer_turn_on(mag_num);
	Sleep(1); // Ждем включения 1 секунду
	printf("Enable Sun sensors 1-4\n");
	sun_sensor_turn_on(1);
	sun_sensor_turn_on(2);
	sun_sensor_turn_on(3);
	sun_sensor_turn_on(4);
	Sleep(1);
	printf("Enable motor №%d\n", mtr_num); 
	motor_turn_on(mtr_num);
	Sleep(1);
}

void switch_off_all(void){/* Функция отключает все приборы,
 которые будут использоваться в основной программе.*/
	printf("Finishing...");
	int16_t new_speed = 0;
	hyro_turn_off(hyr_num);
	magnetometer_turn_off(mag_num);
	sun_sensor_turn_off(1);
	sun_sensor_turn_off(2);
	sun_sensor_turn_off(3);
	sun_sensor_turn_off(4);
	motor_set_speed(mtr_num, 0, &new_speed);
	Sleep(1);
	motor_turn_off(mtr_num);
	printf("\nFinish program\n");
}

int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){
	/*Функция mag_calibrated вносит поправки 
в показания магнитометра с учетом калибровочных коэффициентов
 вместо этих 3-х строк кода с коэффициентами калибровки, должны быть строки с коэффициентами калибровки для Вашего магнитометра
	//magx_cal = 1.04*magx - 0.26*magy + 0.05*magz - 68.76		# это 1-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра
	//magy_cal = 0.24*magx + 1.04*magy + 0.29*magz + 256.92	# это 2-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра
	//magz_cal = -0.09*magx - 0.19*magy + 0.77*magz + 159.41	# это 3-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра*/
	float magx_cal;
	float magy_cal;
	float magz_cal;
	magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24);
	magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24);
	magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24);
	*magx = magx_cal;
	*magy = magy_cal;
	*magz = magz_cal;
	return 0;
}
 
int motor_new_speed_PD(int mtr_speed, float omega, int16_t omega_goal){	
	/* Функция для определения новой скорости маховика.
 Новая скорость маховика складывается из
 текущей скорости маховика и приращения скорости.
 Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости.
 mtr_speed - текущая угловая скорость маховика, об/мин
 omega - текущая угловая скорость спутника, град/с
 omega_goal - целевая угловая скорость спутника, град/с
 mtr_new_speed - требуемая угловая скорость маховика, об/мин*/
	int16_t mtr_new_speed;
	mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal));	
	if (mtr_new_speed > mtr_max_speed)
	{
		mtr_new_speed = mtr_max_speed;
	}
	else if (mtr_new_speed < -mtr_max_speed)
	{
		mtr_new_speed = -mtr_max_speed;
	}
	return mtr_new_speed;
}

int control(){// Основная функция программы, в которой вызываются остальные функции.
	int16_t omega;
	int omega_goal = 0;		// omega_goal - целевая угловая скорость спутника, град/с
	initialize_all();		
	int mtr_state = 0;		// Инициализируем статус маховика	
	int hyro_state = 0;		// Инициализируем статус ДУС
	int mag_state = 0; 		// Инициализируем статус магнитометра
	int16_t mtr_speed;
	int16_t mtr_new_speed;
	//данные ДУС
	int16_t gx_raw;
	int16_t gy_raw;
	int16_t gz_raw;
	int16_t *hyrox_raw=&gx_raw;
	int16_t *hyroy_raw= &gy_raw;
	int16_t *hyroz_raw = &gz_raw;
	//данные магнитометра
	int16_t mgx_cal=0;
	int16_t mgy_cal=0;
	int16_t mgz_cal=0;
	int16_t *magx_raw = &mgx_cal;
	int16_t *magy_raw = &mgy_cal;
	int16_t *magz_raw = &mgz_cal;

	float gx_degs;
	float gy_degs;
	float gz_degs;
	uint16_t sun_result_1[] = {0,0,0}; // Инициализируем sun_result_1
	uint16_t sun_result_2[] = {0,0,0}; // Инициализируем sun_result_2
	uint16_t sun_result_3[] = {0,0,0}; // Инициализируем sun_result_3
	uint16_t sun_result_4[] = {0,0,0}; // Инициализируем sun_result_4
	int mag_alpha = 0;
	const int sizeOD = 10;
    int sizeODA = 0;
    int tempSODA;
//	double* output_data = (double*)calloc(sizeOD, sizeof(double));
    double* output_data_all = (double*)calloc(sizeODA, sizeof(double));
	
	// Номер результата измерений
	int i = 0;
	// Запоминаем время начала вращения
	long int time_start = time(NULL);
	// Запоминаем время начала секундного интервала
	long int time_interval = time(NULL);
	// Интервал вывода данных с солнечных датчиков в секундах
	int time_output = 0.1;
	int j;
	char a=1;
	
	while (a==1){
		// Опрос датчика угловой скорости и маховика.
		hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); 
		mtr_state = motor_request_speed(mtr_num, &mtr_speed);
		mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);

			
		if (!hyro_state){
			 /*Обработка показаний датчика угловой скорости,
		 вычисление угловой скорости спутника по показаниям ДУС.
		 Если код ошибки ДУС равен 0, т.е. ошибки нет*/
			gx_degs = gx_raw * 0.00875; 
			gy_degs = gy_raw * 0.00875;
			gz_degs = gz_raw * 0.00875;
			/* если ДУС установлен осью z вверх, то угловая скорость
			 спутника совпадает с показаниями ДУС по оси z, иначе
			 необходимо изменить знак: omega = - gz_degs*/
			omega = gz_degs;
//			printf("gx_degs=%f, gy_degs=%f, gz_degs=%f\n", gx_degs, gy_degs, gz_degs);//ну так на всякий 
		}
		else if (hyro_state == 1){
			printf("Fail because of access error, check the connection\n");
		}
		else if (hyro_state == 2) {
			printf("Fail because of interface error, check your code\n");
		}

		
		//Обработка показаний маховика и установка требуемой угловой скорости.	
		if (!mtr_state)	{// если код ошибки 0, т.е. ошибки нет
			int16_t mtr_speed=0;
			motor_request_speed(mtr_num, &mtr_speed);
//			printf("Motor_speed: %d\n", mtr_speed);	
			// установка новой скорости маховика
			mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal);
			motor_set_speed(mtr_num, mtr_new_speed, &omega);
		}
	
		Sleep(time_step);
		long int time_current = time(NULL) - time_start;
		
		if (!mag_state){
			mag_calibrated(magx_raw,magy_raw,magz_raw);
			*magy_raw = - *magy_raw; /*переходим из левой системы координат,
			которая изображена на магнитометре в правую, для того чтобы 
			положительное направление угла было против часовой стрелки*/			
			mag_alpha = atan2(mgy_cal, mgx_cal)/M_PI*180;
		}
	
		if ((time(NULL) - time_interval) > time_output){
			// Запоминаем время начала следующего секундного интервала
			time_interval = time(NULL);
			sun_result_1[0] = sun_sensor_request_raw(1, &sun_result_1[1],&sun_result_1[2]);
			sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]);
			sun_result_3[0] = sun_sensor_request_raw(3,&sun_result_3[1],&sun_result_3[2]);
			sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]);
	
			int output_data[] = {time_current, sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2], mag_alpha};
			tempSODA = sizeODA;
			sizeODA += sizeOD;
			output_data_all = (double*)realloc(output_data_all, sizeODA*sizeof(double));
			for (j=tempSODA; j<sizeODA; j++) {
				output_data_all[j] = output_data[j-sizeODA+1];
			}
		}		
		if (i > 100){  // Начинаем вращение через 5с после запуска
			omega_goal = 6.0;  // omega_goal - целевая угловая скорость спутника, град/с
		}
 
		if 	(time_current > 90){
			break;	
		}
		i += 1;	
	} 
	
	switch_off_all();
 
	printf("time_end = %ld" , time(NULL) - time_start);
 
	for (i = 0; i < 5000; i = i + 10){
		printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",output_data_all[i], output_data_all[i+1], output_data_all[i+2], output_data_all[i+3], output_data_all[i+4], output_data_all[i+5], output_data_all[i+6], output_data_all[i+7], output_data_all[i+8], output_data_all[i+9]);
	}
	printf ("Ok\n");
	return 0;
}