// gen_binary_code.c
// 入力ファイル:
//   controller0_mode_r.dat ... ブロック対角可制御正準形の状態空間実現
//                              一行目は，サンプリング周期[sec](このプログ
//                              ラムでは読み飛ばす, その後 A, B, C, D 行列
//                              が格納されていると仮定
//   controller_order_r.dat ... その次数
// 出力ファイル:
//   ax_0.c, ax_1.c ... x[k+1] = A x[k] + y の演算を行うコード
//   cx_0.c, cx_1.c ... u = Cx の演算を行うコード
//   dy.c ... u += Dy の演算を行うコード
//   def.h ... #define N 等を含むヘッダファイル

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define M 10 // コントローラの係数を近似するビット数
//#define M 16 // コントローラの係数を近似するビット数
//#define DEBUG // gen_binary_code() が呼び出されるたび，第一引数(実数)を表示する．
#define SPLIT_A 0.8 // x[k+1] = A x[k] + y のコードを ax_0.c に含める割合
//#define SPLIT_A 0.6 // x[k+1] = A x[k] + y のコードを ax_0.c に含める割合
// 実数 a と整数 x の積を計算する．
// ただし，実数 a を 2 進数表現で小数点以下第 n ビットまで近似する．
void gen_binary_code(float a, int n, char *in, char *out, int add, FILE *fp)
{
	int i, set_count, last_set;
	float y = 1., r = fabs(a);
	char *let1, *let2, me[3] = "-=", pe[3] = "+=", em[3] = "=-", ep[3] = "= ";

#ifdef DEBUG
	fprintf(stderr, "%e\n", a);
#endif
	
	if(r > 2.){
		fprintf(stderr, "gen_binary_code(): Out of range! (r = %f)\n", r);
	}

	/*
	fprintf(fp, "\t\tif(%s < 0){\n", in);
	fprintf(fp, "\t\t\tp = -%s;\n", in);
	fprintf(fp, "\t\t}else{\n");
	fprintf(fp, "\t\t\tp = %s;\n", in);
	fprintf(fp, "\t\t}\n");
	*/

	fprintf(fp, "\t\tif(%s < 0) p = -%s; else p = %s;\n", in, in, in);

	set_count = 0; // 1 がたった回数
	last_set = 0; // 前回 1 がたっていたループカウンタ i の値
	for(i = 0; i <= n; i++){
		if(r > y){
			r -= y;
			if(set_count == 0){
				if(i == 0){
					fprintf(fp, "\t\ttmp = p;\n");
					fprintf(fp, "\t\tsum = tmp;\n");
				}else{
					fprintf(fp, "\t\ttmp = p >> %d;\n", i - last_set);
					fprintf(fp, "\t\tsum = tmp;\n");
				}
				set_count ++;
			}else{
				fprintf(fp, "\t\ttmp >>= %d;\n", i - last_set);
				fprintf(fp, "\t\tsum += tmp;\n");
				//fprintf(fp, "\t\tsum_p_equ_tmp();\n");
			}
			last_set = i;
		}
		y /= 2;
	}

	/*
	fprintf(fp, "\t\tif(%s < 0){\n", in);
	if(a > 0){
		fprintf(fp, "\t\t\t%s -= sum;\n", out);
	}else{
		fprintf(fp, "\t\t\t%s += sum;\n", out);
	}
	fprintf(fp, "\t\t}else{\n");
	if(a > 0){
		fprintf(fp, "\t\t\t%s += sum;\n", out);
	}else{
		fprintf(fp, "\t\t\t%s -= sum;\n", out);
	}
	fprintf(fp, "\t\t}\n");
	*/

	if(add == 1){
		if(a > 0){
			let1 = me;
			let2 = pe;
		}else{
			let1 = pe;
			let2 = me;
		}
	}else{
		if(a > 0){
			let1 = em;
			let2 = ep;
		}else{
			let1 = ep;
			let2 = em;
		}
	}		

	fprintf(fp, "\t\tif(%s < 0){\n", in);
	fprintf(fp, "\t\t\t%s %s sum;\n", out, let1);
	fprintf(fp, "\t\t}else{\n");
	fprintf(fp, "\t\t\t%s %s sum;\n", out, let2);
	fprintf(fp, "\t\t}\n");
}

void print_cx(float *A, float *B, float *C, float D, int n)
{
	char str[5]; // 文字列 'x[0]', 'x[1]', ... を格納する文字変数
	int i, j;
	FILE *fp0, *fp1, *fp;

	fp1 = fopen("cx_1.c", "w"); // set_adc_channel(0) の後 50 μsec 待つための処理
	fp0 = fopen("cx_0.c", "w"); // 上記以外の処理
	fp = fp0;

	fprintf(fp, "\n\t\t// u = C[0] * x[0]\n");
	gen_binary_code(C[0], M, "x[0]", "u", 0, fp);
	for(i = 1; i < n; i++){
		if(i == n-2){
			fp = fp1;
		}
		fprintf(fp, "\n\t\t// u += C[%d] * x[%d]\n", i, i);
		sprintf(str, "x[%d]", i);
		gen_binary_code(C[i], M, str, "u", 1, fp);
	}
				
	close(fp0);
	close(fp1);
}

void print_dy(float *A, float *B, float *C, float D, int n)
{
	FILE *fp;

	fp = fopen("dy.c", "w");

	fprintf(fp, "\n");
	fprintf(fp, "\n\t\t// u += D * y;\n");
	gen_binary_code(D, M, "y", "u", 1, fp);

	fclose(fp);
}	

void print_def(int n)
{
	FILE *fp;

	fp = fopen("def.h", "w");

	fprintf(fp, "#define N %d\n", n);

	fclose(fp);
}

void print_ax(float *A, float *B, float *C, float D, int n)
{
	char str[5]; // 文字列 'x[0]', 'x[1]', ... を格納する文字変数
	int i, j, m = n * SPLIT_A;
	FILE *fp0, *fp1, *fp;

	printf("m = %d\n", m);

	fp0 = fopen("ax_0.c", "w"); // 一ページに収まらないので，分ける
	fp1 = fopen("ax_1.c", "w");

	fp = fp0;

	//	fprintf(fp, "void calc_ax()\n");
	//	fprintf(fp, "{\n");

	for(i = 0; i < n; i++){
		if(i >= m){ // ループの途中で出力先ファイルを ax_0.c から ax_1.c に切替える
			fp = fp1;
		}
		if(i == n-1){ // 最後?
			fprintf(fp, "\n\t\t// x_tmp = A[%d][%d] * x[%d] + y;\n", i, i, i);
			sprintf(str, "x[%d]", i);
			gen_binary_code(A[i*n+i], M, str, "x_tmp", 0, fp);
			fprintf(fp, "\t\tx[%d] = x_tmp + y;\n", i);
		}else{
			if(B[i+1] == 1){ // ブロックのサイズは 1 ?
				fprintf(fp, "\n\t\t// x_tmp = A[%d][%d] * x[%d] + y;\n", i, i, i);
				sprintf(str, "x[%d]", i);
				gen_binary_code(A[i*n+i], M, str, "x_tmp", 0, fp);
				fprintf(fp, "\t\tx[%d] = x_tmp + y;\n", i);
			}else{ // ブロックのサイズは 2.
				fprintf(fp, "\n\t\t// x_tmp = A[%d][%d] * x[%d] + A[%d][%d] * x[%d]\n", i, i, i, i, i+1, i+1);
				sprintf(str, "x[%d]", i);
				gen_binary_code(A[i*n+i], M, str, "x_tmp", 0, fp);
				sprintf(str, "x[%d]", i+1);
				gen_binary_code(A[i*n+i+1], M, str, "x_tmp", 1, fp);
				fprintf(fp, "\t\tx[%d] = x[%d];\n", i+1, i);
				fprintf(fp, "\t\tx[%d] = x_tmp + y;\n", i);

				i++; // 次のブロックに飛ばす
			}
		}
	}
				
	// fprintf(fp, "}\n");

	fclose(fp0);
	fclose(fp1);
}


int main()
{
	int i, j, n;
	FILE *fp, *fp2, *fp0, *fp1;
	float *A, *B, *C, D, dummy;

	//fp = fopen("binary_code.c", "w");
	fp0 = fopen("data/controller0_mode_r.dat", "r");
	fp1 = fopen("data/controller_order_discrete.dat", "r");

	fscanf(fp1, "%d", &n); // 補償器の次数を読み込む
#ifdef DEBUG
	fprintf(stderr, "order=%d\n", n);
#endif
	A = malloc(n * n * sizeof(float));
	B = malloc(n * sizeof(float));
	C = malloc(n * sizeof(float));

	fscanf(fp0, "%f", &dummy); // サンプリング周期

	// 補償器の状態空間実現を読み込む
#ifdef DEBUG
	fprintf(stderr, "A = \n");
#endif
	for(i = 0; i < n; i++){
		for(j = 0; j < n; j++){
			fscanf(fp0, "%f", &A[i*n+j]);
#ifdef DEBUG
			fprintf(stderr, "%e ", A[i*n+j]);
#endif			
		}
#ifdef DEBUG
		fprintf(stderr, "\n");
#endif
	}
	for(i = 0; i < n; i++){
		fscanf(fp0, "%f", &B[i]);
	}
	for(i = 0; i < n; i++){
		fscanf(fp0, "%f", &C[i]);
	}
	fscanf(fp0, "%f", &D);

	print_ax(A, B, C, D, n);
	print_cx(A, B, C, D, n);
	print_dy(A, B, C, D, n);
	print_def(n);


	return 0;
}