#include"SensorIS3.h"

#include <Communication_Protocol.h>

#include "iostream"
#include "IRIS_Method.h"

#include <cstring>

static void split(const std::string& s, std::vector<std::string>& sv, char flag)
{
    sv.clear();
    std::istringstream iss(s);
    std::string temp;

    while (getline(iss, temp, flag)) {
        sv.push_back(temp);
    }
    return;
}


bool SensorIS3::initSensor(int id)
{

    DataRetrun=GetDataBufferPTR();
   // IS1Sensor.SetPortName(id);
    SensorInfo=GetSensorInfo();
//    pinMode(22,OUTPUT);
//    pinMode(23,OUTPUT);
 
    return true;
}

STRSensorInfo SensorIS3::GetSensorInfo()
{

     STRSensorInfo setem;
    if (!isSensorInit())
    {

        return setem;
    }


     int retlenth= SendGetSensorInfo(GET_BASIC_INFO,sizeof(GET_BASIC_INFO));

    char * result=new char[100];
    memcpy(result,DataRetrun,retlenth);
    result[retlenth]='\0';
    std::cout<<result<<std::endl;
    std::string str11(result);
    std::vector<std::string> strvec;
    split(str11,strvec,'-');
    setem.SensorName=strvec[0];

    if (strvec.size()>=2) {
        setem.serialnumber=strvec[1];
    }
     delete[] result;
      
    retlenth= SendGetSensorInfo(GET_BANDNUMBER,sizeof(GET_BASIC_INFO));
    int bandnumber=0;
    memcpy(&bandnumber,DataRetrun,retlenth);
    CoverLittleandBigbyNumber(&bandnumber,4);
    setem.BandNum=bandnumber;

    setem.maxValue=65535;

    setem.wavelenthlist=new float[setem.BandNum]();
     retlenth= SendGetSensorInfo(GET_WEAVE_COEFF,sizeof(GET_WEAVE_COEFF));
    double a[4];
    char tempaa[100];
    memcpy(tempaa,DataRetrun,100);
    int lenthtemp=sizeof(double);
     memcpy(a,DataRetrun,4*sizeof(double));

    CoverLittleandBigbyNumber(a,sizeof(double));
    CoverLittleandBigbyNumber(a+1,sizeof(double));
    CoverLittleandBigbyNumber(a+2,sizeof(double));
    CoverLittleandBigbyNumber(a+3,sizeof(double));
    // a[0]=0;
    // a[1]=0;
    // a[2]=1;
    // a[3]=350;



   // CoverLittleAndBig((char *)a,4*sizeof(float));
    int bandsss= setem.BandNum;
    #ifdef ARDUINO
  
    Serial.print("a0:");
    Serial.print(a[0]);
    Serial.print("  a1:");
    Serial.print(a[1]);
    Serial.print("  a2:");
    Serial.print(a[2]);
    Serial.print("  a3:");
    Serial.println(a[3]);
    #endif
  
    std::cout<<"a0:"<<a[0]<<"  a1:"<<a[1]<<"  a2:"<<a[2]<<"  a3:"<<a[3]<<std::endl;
   
    setem.a1=a[0];
    setem.a2=a[1];
    setem.a3=a[2];
    setem.a4=a[3];
    for ( int i = 1; i <=bandsss ; i++)
    {
        setem.wavelenthlist[i-1] = a[0] * i*i*i + a[1] * i*i + a[2] * i + a[3];
        setem.WavelenthStr=setem.WavelenthStr+std::to_string( setem.wavelenthlist[i-1])+",";
    }
    //  delay(500);
    // Serial.write(dataretrun,retlenth);
   // memcpy(a,dataretrun+) 
   u_char temp[2]={0x00,0x01};
    // retlenth=SendSettingCommand((u_char*)SET_AVERAGE_NUMBER,sizeof(SET_AVERAGE_NUMBER),temp,sizeof(temp));
//    Serial.println("init ok");

    return setem;

    
}

void SensorIS3::SetShutter(int id)
{
#ifdef ARDUINO
    switch (id) {
        case 0:
        {
            digitalWrite(22,LOW);
            digitalWrite(23,LOW);
            delay(200);
            break;
        }
        case 1:{
            digitalWrite(23,HIGH);
            digitalWrite(22,LOW);
            delay(200);
            break;
        }
        case 2:{
            digitalWrite(22,HIGH);
            digitalWrite(23,LOW);
            delay(200);
            break;
        }
    }
#else
    switch (id) {

        case 0: {
            u_char a=1;
            SendSettingCommand(SET_SHUTTER_CLOSE,1, &a, 1);
            break;
        }
        case 1: {
            u_char a=1;
            //SendSettingCommand(SET_SHUTTER_CLOSE,1, &a, 1);
            SendSettingCommand(SET_SHUTTER_OPEN,1, &a, 1);
            break;
        }

    }

#endif
}

void SensorIS3::SetWeaveLenthCoeff(double *a, int lenth) {
    u_char *datetosend=new u_char[lenth*sizeof(double)];
    printf("c++:a0:%f  a1:%f  a2:%f  a3:%f\n",a[0],a[1],a[2],a[3]);
    CoverLittleandBigbyNumber(a,sizeof(double));
    CoverLittleandBigbyNumber(a+1,sizeof(double));
    CoverLittleandBigbyNumber(a+2,sizeof(double));
    CoverLittleandBigbyNumber(a+3,sizeof(double));

    memcpy(datetosend,a,lenth*sizeof(double));
    SendSettingCommand(SET_WEAVE_COEFF,1,datetosend,lenth*sizeof(double));
    delete[] datetosend;
   // GetSensorInfo();
}

void SensorIS3::GetOneDate(int msc)
{
 
    if (msc!=shutternow)
    {
           u_char shutter[4];
            memcpy(shutter,&msc,4);
            //交换下shutter 前后
        u_char shutterlitte[4];
        shutterlitte[0]=shutter[3];
        shutterlitte[1]=shutter[2];
        shutterlitte[2]=shutter[1];
        shutterlitte[3]=shutter[0];







       int ret= SendSettingCommand(SET_INTEGRAL_TIME,1,shutterlitte,sizeof(shutterlitte));


    }
    shutternow=msc;
   size_t retsize= SendGetData(shutternow);

   memcpy(DATABUFF,DataRetrun,515*2);
   //shortLittletoBiG(DATABUFF, SensorInfo.BandNum*2);
    


    
}

int SensorIS3::OptSnenser(int persent)
{
    long minshuttertime=12;
    long maxtime = 10000;
    int maxvalue=SensorInfo.maxValue*1.0*persent / 100;
    int maxvaluenow = 0;
    float shutternow = 20;
    GetOneDate(shutternow);
    maxvaluenow= Getmaxvalue(DATABUFF, SensorInfo.BandNum);

    int numberoftry = 0;
    while (maxvaluenow<maxvalue*0.95 || maxvaluenow>maxvalue) {

        if (maxvaluenow > maxvalue)
        {
            shutternow = shutternow *0.7;
        }
        else if (maxvaluenow==0)
        {
            shutternow =shutternow*10;
        }
        else
        {
            shutternow = maxvalue * 0.98 / (maxvaluenow * 1.0)*shutternow + 1;
        }
        if (shutternow > maxtime)
        {
            shutternow = maxtime;
            break;
        }
        if (shutternow < minshuttertime)
        {
            shutternow = minshuttertime;
            break;
        }
        GetOneDate(shutternow);
        maxvaluenow= Getmaxvalue(DATABUFF, SensorInfo.BandNum);
#ifdef ARDUINO
        Serial.print("now Shutter is :");
        Serial.print(shutternow);
        Serial.print("  maxvalue is :");
        Serial.println(maxvaluenow);
#else
        std::cout<<"now Shutter is :"<<shutternow<<"  maxvalue is :"<<maxvaluenow<<std::endl;
#endif
        numberoftry++;
        if (numberoftry > 200)
        {

            return maxtime;

        }
        if (shutternow == maxtime)
        {

            return  maxtime;
        }
    }
#ifdef ARDUINO
    Serial.print("zi dong value:");
    Serial.println(shutternow);
#endif
    if (shutternow<0)
    {
        shutternow=maxtime;
        /* code */
    }

    return shutternow;
}

void SensorIS3::shortLittletoBiG(unsigned short *data,int lenth)
{
    CoverLittleAndBig((char *)data,lenth);
}











int SensorIS3::Getmaxvalue(unsigned short *data,int lenth)
{
    int ret=-1;
    for (int i = 0; i < lenth; ++i) {
        if (data[i]>ret){
            ret=data[i];
        }
    }
    return ret;
}

void SensorIS3::TakeOneJob()
{
        
     SetShutter(1);
    int shutter=OptSnenser(90);

    shutterup=shutter;
    GetOneDate(shutter);
       //  Serial.println("Finish Up ------Green2313123");
         if (UpData==nullptr)
         {
            UpData=new unsigned short[SensorInfo.BandNum*2];
            /* code */
         }
         
    memcpy(UpData,DATABUFF,SensorInfo.BandNum*2);
    SetShutter(0);
    GetOneDate(shutter);
    for (int i = 0; i < SensorInfo.BandNum; ++i) {
      //  UpData[i]=UpData[i]-DATABUFF[i];
    }
    shutterup=shutter;

    bool dingbing=false;
#ifdef DINBIAO
    String strout="1 shutteruo is "+String(shutterup)+" Finish Up ------Green change the fiber";
    PrintFunc(strout );
        digitalWrite(21, LOW);
    delay(60000);
    SensorInfo.SensorName=SensorInfo.SensorName+"cali";
    Serial.println("begindown");
        digitalWrite(21, HIGH);
#endif
    
   

    SetShutter(2);
     shutter=OptSnenser(90);
     shutterdown=shutter;
    GetOneDate(shutter);
         if (DownData==nullptr)
         {
            DownData=new unsigned short[SensorInfo.BandNum*2];
            /* code */
         }
    memcpy(DownData,DATABUFF,SensorInfo.BandNum*2);
    SetShutter(0);
    GetOneDate(shutter);


    for (int i = 0; i < SensorInfo.BandNum; ++i) {
      //  DownData[i]=DownData[i]-DATABUFF[i];
    }
    shutterdown=shutter;

#ifdef DINBIAO
        String strout1="2 shutterdown is "+String(shutterdown)+" Down Finish ------Blue";
        PrintFunc(strout1 );

#endif
}

SensorIS3::SensorIS3()
{
    shutternow=0;
   // DataRetrun=GetDataBufferPTR();
}
