#include <decision/obs_predict.h>
#include <decision/decition_type.h>
#include <common/gps_type.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <decision/adc_tools/transfer.h>
#include "perception/perceptionoutput.h"
#include<common/constants.h>

using namespace std;
#define PI (3.1415926535897932384626433832795)


double iv::decition::PredictObsDistance( double realSpeed,std::vector<Point2D>  gpsTrace,std::vector<iv::Perception::PerceptionOutput> lidar_per){

    double crashDistance=200;
    for(int i=0;i<lidar_per.size();i++){
        if(lidar_per[i].life>300 && (lidar_per[i].velocity.x!=0 || lidar_per[i].velocity.y!=0)){
            if(lidar_per[i].location.y>20 && lidar_per[i].velocity.y>0){

            }
            else if(lidar_per[i].location.y<0 && lidar_per[i].velocity.y<0){

            }
            else if(lidar_per[i].location.x>20 && lidar_per[i].velocity.x>0){

            }else if(lidar_per[i].location.x<-20 && lidar_per[i].velocity.x<0){

            }else{

                double    crashDis = getCrashDis(realSpeed,gpsTrace,lidar_per[i]);

                crashDistance=min(crashDis,crashDistance);
            }


        }

    }





    return crashDistance;


}

double iv::decition::getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace,iv::Perception::PerceptionOutput obs){
    double dis=0;
    int length=min(300,(int)gpsTrace.size());
    int step=10;
    int size =length/step;
    for(int i=1; i<size;i++){
        double time =  getTime( realSpeed,  gpsTrace, i*step,&dis);
        double obsX= obs.location.x+obs.velocity.x*time;
        double obsY= obs.location.y+obs.velocity.y*time;
        Point2D obsP(obsX,obsY);
        double obsDis= GetDistance(obsP,gpsTrace[i*10]);
        if(obsDis<0.7*(ServiceCarStatus.msysparam.vehWidth+obs.size.x) ){
            return dis;
        }

    }

    return 200;

}



int iv::decition::getFrameCount(double realSpeed,std::vector<Point2D>  gpsTrace){
    double lenth=0;
    for(int i =0 ; i<gpsTrace.size()-1;i++){
        lenth +=GetDistance(gpsTrace[i],gpsTrace[i+1]);
    }

    int frame= lenth/realSpeed;
    frame = min(frame,50);

    return frame;
}

int  iv::decition::getPoiIndex(double realSpeed,std::vector<Point2D>  gpsTrace,int frame){



    double d =realSpeed*0.1*frame;
    int index=0;

    for(int j=0;j<gpsTrace.size()-1;j++){
        double dis;
        dis+=GetDistance(gpsTrace[j],gpsTrace[j+1]);
        if(dis>d){
            index=j;
            break;
        }
        index=j;
    }

    return index;
}

double iv::decition::getTime(double realSpeed,std::vector<Point2D>  gpsTrace,int frame ,double *dis){
    int size=gpsTrace.size()-1;
    int f=min(frame,size);

    for(int i=0;i<=f;i++){

        *dis+=GetDistance(gpsTrace[0],gpsTrace[f]);

    }
    double time = *dis/realSpeed;
    return time;

}