//
// Created by Wakanda_shaw on 2022/11/2.
//

#include "rtklib.h"

#define MINVEL        5.0             /* min velocity for initial ins states */
#define MAXGYRO       (30.0*D2R)      /* max rotation speed value for initial */
#define MICRO_G_2_MS  9.80665E-6      /* micro_g_to_meters_per_second_squared */

/* coarse alignment for ins navigation initial with stationaly imu measurement data
 * args  : insstate_t *ins  IO  ins states
 *         imud_t *data     I   imu measurement data (static case) {m/s^2,rad/s}
 *         int    n         I   number of data (0: no use of data)
 *         insopt_t  *opt   I   ins options
 * return : 1:ok, 0:fail
 * note   : this model is suitable for small attitude error's
 * ---------------------------------------------------------------------------*/
static int coarse_align(ins_t *ins,const imud_t *data,int n){
    int i,j,k;
    double pos[3],fb[3]={0},omg[3]={0},Cne[9],vn[3];
    double roll,pitch,yaw;

    trace(3,"coarse_asign: n=%d\n",n);

    if (n<=0) return 0;
    ecef2pos(ins->position,pos);
    ned2xyz(pos,Cne);

    matmul("TN",3,1,3,1.0,Cne,ins->velocity,0.0,vn);
    yaw = vel2head(vn); /* yaw */

    /* average imu measurement data for remove noise */
    for (k=0,i=0;i<n;i++) {
        for (k++,j=0;j<3;j++) {
            fb[j]+=data[i].accl[j];
            omg[j]+=data[i].gyro[j];
        }
    }
    for (i=0;i<3;i++) fb[i]/=k,omg[i]/=k;

    roll = atan2(-fb[1],-fb[2]);
    pitch = atan(fb[0]/ sqrt(fb[1]*fb[1] + fb[2]*fb[2]));


    ins->Cbn[0] = cos(pitch)* cos(yaw);
    ins->Cbn[1] = cos(pitch) * sin(yaw);
    ins->Cbn[2] = -sin(pitch);
    ins->Cbn[3] = -cos(roll)* sin(yaw) + sin(roll)* sin(pitch) * cos(yaw);
    ins->Cbn[4] = cos(roll) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
    ins->Cbn[5] = sin(roll) * cos(pitch);
    ins->Cbn[6] = sin(roll) * sin(yaw) + cos(roll) * sin(pitch) * cos(yaw);
    ins->Cbn[7] = -sin(roll) * cos(yaw) + cos(roll) * sin(pitch) * sin(yaw);
    ins->Cbn[8] = cos(roll) * cos(pitch);

    /* normalization */
    matmul("NN",3,3,3,1.0,Cne,ins->Cbn,1.0,ins->Cbe);
    normdcm(ins->Cbe);

    return 1;
}

/* initialize ins states use PVT---------------------------
 * args:
 *          ins_t       *ins        IO  ins_states
 *          pvt_t       *pvt        I   pvt states
 *          imu_t       *imu        I   imu data
 * return:  imu_index
 * ------------------------------------------------*/
extern int ins_init_pvt(ins_t *ins, pvt_t *pvt, imu_t *imu){
    int i;
    int imu_index=0;
    double vr[3]={0};

    for(i=0;i<pvt->n-1;i++){
        /* init ins time */
        ins->time = pvt->data[i].tow;
        /* align imu data */
        while(ins->time!=imu->data[imu_index].tow){
            imu_index++;
        }
        ins->imu_time = imu->data[imu_index].time;
        for(int k=0;k<3;k++){
            ins->fb[k] = imu->data[imu_index].accl[k];
            ins->omgb[k] = imu->data[imu_index].gyro[k];
        }
        matcpy(vr,pvt->data[i].ecef_v,1,3);
        if (norm(vr,3)<MINVEL) {
            printf("velocity = %lf\n", norm(vr,3));
            continue;
        }
        /* check velocity is ok */
        break;
    }
    if(i==pvt->n){
        return -1;
    }

    for(int k=0;k<3;k++){
        ins->velocity[k]=pvt->data[i].ecef_v[k];
        ins->position[k]=pvt->data[i].ecef_pos[k];
        ins->acc_bias[k]=0;
        ins->gyro_bias[k]=0;
    }

    /* initial imu attitude */
    if(!coarse_align(ins,imu->data,1000)){
        trace(3, "coarse align fail\n");
    }

    return imu_index;
}