#include"SensorIS11.h"
#include "iostream"

#ifdef _WIN32
#include <windows.h>
#else

#include <unistd.h>
#include <cstring>
#endif

bool SensorIS11::initSensor(int id)
{

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

STRSensorInfo SensorIS11::GetSensorInfo()
{

     STRSensorInfo setem;
    if (!isSensorInit())
    {

        return setem;
    }


    int retlenth= SendGetSensorInfo((u_char *)GET_SERIAL_NUMBER,sizeof(GET_SERIAL_NUMBER));

    char * result=new char[5];
    memcpy(result,DataRetrun+3,4);
    result[4]='\0';
    std::cout<<result<<std::endl;
    std::string str11(result);
     setem.serialnumber=str11;
     delete[] result;
    //Serial.println(setem.serialnumber);
      
    retlenth= SendGetSensorInfo((u_char *)GET_PRODUCT_NAME,sizeof(GET_PRODUCT_NAME));
    result=new char[5];
    memcpy(result,DataRetrun+3,4);
   // delay(500);
    
    result[4]='\0';
     std::string str1(result);
     setem.SensorName=str1;
     delete result;
     setem.BandNum=2048;

    setem.maxValue=65535;

    setem.wavelenthlist=new float[setem.BandNum]();
    retlenth= SendGetSensorInfo((u_char *)GET_WAVELENTH_COEFF,sizeof(GET_WAVELENTH_COEFF));
    float a[4];
    memcpy(a,DataRetrun+3,16); 

    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 SensorIS11::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;
        }
    }
#endif
}

void SensorIS11::GetOneDate(int msc)
{
 
    if (msc!=shutternow)
    {
           u_char shutter[2];
           shutter[0]=msc/256;
           shutter[1]=msc%256;
        SendSettingCommand((u_char *) SET_INTEGRAL_TIME,sizeof(SET_INTEGRAL_TIME),shutter,sizeof(shutter));
    
    }
    shutternow=msc;
   size_t retsize= SendGetData(shutternow);
   memcpy(DATABUFF,DataRetrun+4,4096);
   shortLittletoBiG(DATABUFF, SensorInfo.BandNum*2);
    


    
}

int SensorIS11::OptSnenser(int persent)
{
     long maxtime = 20000;
    int maxvalue=SensorInfo.maxValue*1.0*persent / 100;
    int maxvaluenow = 0;
    float shutternow = 10;
    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
        {
            shutternow = maxvalue * 0.98 / (maxvaluenow * 1.0)*shutternow + 1;
        }
        if (shutternow > maxtime)
        {
            shutternow = maxtime;
            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 SensorIS11::shortLittletoBiG(unsigned short *data,int lenth)
{
    CoverLittleAndBig((char *)data,lenth);
}











int SensorIS11::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 SensorIS11::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
}

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