#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <GL/glut.h>

#include "matrix.h"

#ifndef M_PI
#define M_PI 3.1415926535F
#endif

typedef struct parameter {
  double value;
  double speed;
  int steps; } Parameter;

Matrix pos;
Parameter roll,alignment,heading,pitch;
double tot_al = 0.0;
int tot = 0;

#define MAX 10000

extern GLfloat x[MAX],y[MAX],z[MAX];
extern GLfloat dx[MAX],dy[MAX],dz[MAX];
extern GLfloat hd[MAX],al[MAX],pt[MAX],rl[MAX];
extern GLfloat strips[27][MAX][3],normal[24][MAX][3];
extern int opt[MAX];
extern GLfloat bnormal[2][MAX][3];
extern GLfloat r1[MAX],r2[MAX],r3[MAX];

void update_parameters(void);
void update_parameter(Parameter *p);
void init_parameter(Parameter *p);

Vector strips_tmp[27],normal_tmp[27];

void calculate_rc(void) {
  FILE *in;
  int i,j;
  char cmd[256];
  double a;
  Vector v;
  Matrix tmp,t2;
#if 0
  GLfloat nx,ny,nz,ac;
#endif
  printf("Reading RC parameters.\n");
  in = fopen("rc.def","r");
  if (!in) {
    fprintf(stderr,"Failed to open file 'rc.def'.\n");
    exit(1); }
  init_parameter(&roll);
  init_parameter(&pitch);
  init_parameter(&heading);
  init_parameter(&alignment);
  init_matrix(&pos);
  pos.index[3][2] = 0;
  for (i=0;i<27;i++) {
    init_vector(&normal_tmp[i]);
    init_vector(&strips_tmp[i]); }
  for (i=0;i<8;i++) {
    strips_tmp[i].index[2] = 1.0;
    strips_tmp[i+8].index[2] = -1.0;
    strips_tmp[i+16].index[1] = -1.0; }
  for (i=0;i<3;i++) {
    for (j=0;j<8;j++) {
      normal_tmp[i*8+j].index[2] = cos(j*M_PI/4);
      normal_tmp[i*8+j].index[1] = sin(j*M_PI/4);
      strips_tmp[i*8+j].index[2] += cos(j*M_PI/4)/(i==2?2:4);
      strips_tmp[i*8+j].index[1] += sin(j*M_PI/4)/(i==2?2:4); }
    strips_tmp[24].index[2] = 1.0;
    strips_tmp[25].index[2] = -1.0;
    strips_tmp[26].index[1] = -1.0; }
  while (!feof(in)) {
    for (i=0;i<256;i++) {
      int ch;
      ch = fgetc(in);
      if ((ch == '\n') || (ch == EOF)) {
        cmd[i] = 0;
        break; }
      cmd[i] = ch; }
    if (cmd[0] == '#')
      continue;
      else
      if (!cmd[0])
        continue;
        else
        if (sscanf(cmd,"pitch %lf %d",&a,&i)) {
          pitch.speed = (a - pitch.value)/i;
          pitch.steps = i; }
          else 
          if (sscanf(cmd,"alignment %lf %d",&a,&i)) {
            alignment.speed = (a - alignment.value)/i;
            alignment.steps = i; }
            else
            if (sscanf(cmd,"heading %lf %d",&a,&i)) {
              heading.speed = (a - heading.value)/i;
              heading.steps = i; }
              else
              if (sscanf(cmd,"roll %lf %d",&a,&i)) {
                roll.speed = (a - roll.value)/i;
                roll.steps = i; }
                else 
                if (sscanf(cmd,"wait %d",&i)) {
                  for (;i>=0;i--) {
                    update_parameters();
                    init_vector(&v);
                    v.index[0] = 0.15;
                    multiply_matrix_vector(&pos,&v);
                    for (j=0;j<3;j++)
                      pos.index[3][j] = v.index[j];
                    rotate_x(-roll.value*M_PI/(180*50),&pos);
                    rotate_y(-heading.value*M_PI/(180*50),&pos);
                    rotate_z(-pitch.value*M_PI/(180*50),&pos);
                    x[tot] = v.index[0];
                    y[tot] = v.index[1];
                    z[tot] = v.index[2];
                    al[tot] = alignment.value/50.0;
                    rl[tot] = roll.value/50.0;
                    hd[tot] = heading.value/50.0;
                    pt[tot] = pitch.value/50.0;
                    opt[tot] = 100*fabs(rl[tot] - al[tot]) + 100*fabs(hd[tot])+100*fabs(pt[tot]);
                    copy_matrix(&tmp,&pos);
                    init_vector(&v);
                    v.index[1] = 1;
                    tot_al += alignment.value*M_PI/180/50;
                    rotate_x(-tot_al,&tmp);
                    multiply_matrix_vector(&tmp,&v);
                    dx[tot] = v.index[0] - tmp.index[3][0];
                    dy[tot] = v.index[1] - tmp.index[3][1];
                    dz[tot] = v.index[2] - tmp.index[3][2];
                    copy_matrix(&tmp,&pos);
                    tot_al += alignment.value*M_PI/180/50;
                    rotate_x(-tot_al,&tmp);
                    copy_matrix(&t2,&tmp);
                    for (j=0;j<27;j++) {
                      copy_vector(&v,&strips_tmp[j]);
                      multiply_matrix_vector(&t2,&v);
                      strips[j][tot][0] = v.index[0];
                      strips[j][tot][1] = v.index[1];
                      strips[j][tot][2] = v.index[2];
                      copy_vector(&v,&normal_tmp[j]);
                      multiply_matrix_vector(&t2,&v);
                      normal[j][tot][0] = v.index[0] - t2.index[3][0];
                      normal[j][tot][1] = v.index[1] - t2.index[3][1];
                      normal[j][tot][2] = v.index[2] - t2.index[3][2]; }
                    init_vector(&v);
                    v.index[0] = -1.0;
                    v.index[1] = -1.5;
                    multiply_matrix_vector(&pos,&v);
                    bnormal[0][tot][0] = v.index[0] - pos.index[3][0];
                    bnormal[0][tot][1] = v.index[1] - pos.index[3][1];
                    bnormal[0][tot][2] = v.index[2] - pos.index[3][2];
                    init_vector(&v);
                    v.index[2] = -1.0;
                    v.index[1] = 1.5;
                    multiply_matrix_vector(&pos,&v);
                    bnormal[0][tot][0] = v.index[0] - pos.index[3][0];
                    bnormal[0][tot][1] = v.index[1] - pos.index[3][1];
                    bnormal[0][tot][2] = v.index[2] - pos.index[3][2];
#if 0
                    copy_matrix(&tmp,&pos);
                    tmp.index[3][0] = 0.0;
                    tmp.index[3][1] = 0.0;
                    tmp.index[3][2] = 0.0;
                    init_vector(&v);
                    v.index[0] = 1.0;
                    multiply_matrix_vector(&tmp,&v);
                    nx = v.index[0];
                    ny = v.index[1];
                    nz = v.index[2];
                    ac = sqrt(nx*nx+nz*nz);
                    if (ac == 0.0)
                      r1[tot] = 0.0;
                      else 
                      if (nx > 0)
                        r1[tot] = asin(nz/ac);
                        else
                        r1[tot] = M_PI-asin(nz/ac);

                    r2[tot] = asin(ny);
                    rotate_y(-r1[tot],&tmp);
                    rotate_z(-r2[tot],&tmp);
                    init_vector(&v);
                    v.index[1] = 1.0;
                    multiply_matrix_vector(&tmp,&v);
                    nx = v.index[0];
                    ny = v.index[1];
                    nz = v.index[2];
                    ac = sqrt(nz*nz+ny*ny);
                    if (ac == 0.0)
                      r3[tot] = 0.0;
                      else 
                      if (nz > 0)
                        r3[tot] = M_PI-asin(ny/ac);
                        else
                        r3[tot] = asin(ny/ac);
#endif
                    copy_matrix(&tmp,&pos);
                    rotate_x(-tot_al,&tmp);
                    if (tmp.index[0][0] == 0.0 && tmp.index[0][1] == 0.0) {
                      r1[tot] = atan2(- tmp.index[1][0],- tmp.index[2][0]);
                      r2[tot] = 0.5F * M_PI;
                      r3[tot] = 0.0F; } 
                      else {
                      r1[tot] = atan2(tmp.index[1][2],tmp.index[2][2]);
                      r2[tot] = asin(tmp.index[0][2]);
                      r3[tot] = atan2(tmp.index[0][1],tmp.index[0][0]); }
#if 0
                    printf("R: %f,%f,%f.\n",r1[tot],r2[tot],r3[tot]);
#endif
                    tot++; } }
      else
      fprintf(stderr,"Not understood: %s\n",cmd); }
  printf("Done.\nTotal of %d parts\n",tot);
  printf("Ended at %f,%f,%f\n",x[tot-1],y[tot-1],z[tot-1]); 
}

void update_parameters(void) {
  update_parameter(&roll);
  update_parameter(&pitch);
  update_parameter(&heading);
  update_parameter(&alignment);
}

void update_parameter(Parameter *p) {
  if (!p->steps)
    return;
  p->steps--;
  p->value += p->speed;
}

void init_parameter(Parameter *p) {
  p->value = 0;
  p->speed = 0;
  p->steps = 10;
}