//
// Created by xin on 2022/4/24.
//

#include "wifidebug.h"

#include <esp_task_wdt.h>
#include "SensorOptoSky.h"
bool SensorOptoSky::initSensor(int id )
{
 

    // pinMode(36,INPUT);
delay(1000);
   pinMode(36,OUTPUT);
    digitalWrite(36,HIGH);
;
    IS1Sensor.SetPortName(id);
    SensorInfo=GetSensorInfo();
    pinMode(5,OUTPUT);
    pinMode(6,OUTPUT);
   // Serial.println("i am here12312312312312312");
    return true;
}

STRSensorInfo SensorOptoSky::GetSensorInfo() {
    STRSensorInfo setem;
  
    if (!IS1Sensor.isSensorInit())
    {
        Serial.println("sensor is 123123");
        return setem;
    }
    int lenthretrunl;
    char *result;

    IS1Sensor.SendComment(GET_PN_NUMBER);
    IS1Sensor.GetReturn(GET_PN_NUMBER, lenthretrunl,1000);
    result=new char[lenthretrunl+1]();
    memcpy(result,IS1Sensor.result,lenthretrunl);
    result[lenthretrunl]='\0';

    String str(result);
    Serial.println(str);
    delete[] result;
    str.remove(str.indexOf("00")+1,str.length()-(str.indexOf("00")+1));
    setem.SensorName=str;
    if(setem.SensorName=="")
    {
        setem.SensorName=="ATP1010";
    }
#ifdef DINBIAO

    setem.SensorName="ATP1010";
    #endif

    /////////////////////////////////////////////////////////

    IS1Sensor.SendComment(GET_SN_NUMBER);
    IS1Sensor.GetReturn(GET_SN_NUMBER, lenthretrunl,1000);
    result=new char[lenthretrunl+1]();
    memcpy(result,IS1Sensor.result,lenthretrunl);
    result[lenthretrunl]='\0';
    String SNNUMBER(result);
    Serial.println(SNNUMBER);
    delete[] result;//
    SNNUMBER.remove(SNNUMBER.indexOf('X'),SNNUMBER.length()-SNNUMBER.indexOf('X'));
    setem.serialnumber=SNNUMBER;
/*
    Serial.write("IS1Sensor.result+10,lenthretrunl");
    Serial.write(IS1Sensor.result+3,lenthretrunl-3);
    Serial.println("SNNUMBER");
    Serial.println(SNNUMBER);

*/
    ////////////////////////


    u_char *shutter=new u_char[2]();
    int msc=10;
	shutter[0] = (int)msc / 256;
	shutter[1] = (int)msc % 256;
    IS1Sensor.SendComment(SYNCHRONIZATION_GET_DATA,shutter,2);
    IS1Sensor.GetReturn(SYNCHRONIZATION_GET_DATA, lenthretrunl,10);
    setem.BandNum = (lenthretrunl-1)/2;
    Serial.println("band lenth:" );
    Serial.println( setem.BandNum );
    DATABUFF=new unsigned short[setem.BandNum] ;
    DownData=new unsigned short[setem.BandNum] ;
    UpData=new unsigned short[setem.BandNum] ;
////////////////////////////////////////////////////////
    shutter[0] = 0x00;
    shutter[1] = 0x01;
    IS1Sensor.SendComment(GET_WAVELENGTH_CALIBRATION_COFF,shutter,2);
    IS1Sensor.GetReturn(GET_WAVELENGTH_CALIBRATION_COFF, lenthretrunl);
    float a[4];
    memcpy(a, IS1Sensor.result + 16, 4 * 4);

    int bandsss= setem.BandNum;
    setem.wavelenthlist=new float[setem.BandNum]();
    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]);
    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];

    }
    ///////////////////////////////////////////////////
    shutter[0] = 0x00;
    shutter[1] = 0x01;

    IS1Sensor.SendComment(SET_AVERAGE_NUMBER, shutter,2);
    IS1Sensor.GetReturn(SET_AVERAGE_NUMBER, lenthretrunl);
    setem.maxValue=65535;
    delete[] shutter;

    return setem;
    /* 非线性校正
    IS1Sensor.SendComment(0x53, shutter,2);
    IS1Sensor. GetReturn(0x53, lenthretrunl);
*/

}

void SensorOptoSky::SetShutter(int id) {
    switch (id) {
        case 0:
        {
            digitalWrite(5,LOW);
            digitalWrite(6,LOW);
            delay(200);
            break;
        }
        case 1:{
            digitalWrite(6,HIGH);
            digitalWrite(5,LOW);
            delay(200);
            break;
        }
        case 2:{
            digitalWrite(5,HIGH);
            digitalWrite(6,LOW);
            delay(200);
            break;
        }
    }

}

void SensorOptoSky::GetOneDate(int msc) {
    u_char *shutter=new u_char[2]();
    shutter[0] = (int)msc / 256;
    shutter[1] = (int)msc % 256;
    int lenthretrunl;
    IS1Sensor.SendComment(SYNCHRONIZATION_GET_DATA,shutter,2);
    IS1Sensor.GetReturn(SYNCHRONIZATION_GET_DATA, lenthretrunl,msc);
    memcpy(DATABUFF, IS1Sensor.result + 1, SensorInfo.BandNum * 2);
    //memcpy(DATABUFF, IS1Sensor.result , SensorInfo.BandNum * 2);
    shortLittletoBiG(DATABUFF, SensorInfo.BandNum);
     esp_task_wdt_reset(); //手动喂狗
}

void SensorOptoSky::shortLittletoBiG(unsigned short *data, int lenth) {
    char *chardata=(char *)data;
    char temp;
    for (int i = 0; i <lenth ; ++i) {
        temp=chardata[2*i+1];
        chardata[2*i+1]=chardata[2*i];
        chardata[2*i]=temp;
    }
}

int SensorOptoSky::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);
    Serial.print("now Shutter is :");
    Serial.print(shutternow);
    Serial.print("  maxvalue is :");
    Serial.println(maxvaluenow);
    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);
        Serial.print("now Shutter is :");
        Serial.print(shutternow);
        Serial.print("  maxvalue is :");
        Serial.println(maxvaluenow);
        numberoftry++;
        if (numberoftry > 200)
        {

            return maxtime;

        }
        if (shutternow == maxtime)
        {

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

int SensorOptoSky::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 SensorOptoSky::TakeOneJob() {
    SetShutter(1);
    int shutter=OptSnenser(90);
    shutterup=shutter;
    GetOneDate(shutter);

     //esp_task_wdt_reset(); //手动喂狗

    memcpy(UpData,DATABUFF,SensorInfo.BandNum*2);
    SetShutter(0);
    GetOneDate(shutter);
    
    for (int i = 0; i < SensorInfo.BandNum; ++i) {
        int temp=UpData[i]-DATABUFF[i];
        UpData[i]=temp>0?temp:0;
        // UpData[i]=UpData[i]-DATABUFF[i];
    }
    shutterup=shutter;
    Serial.println("Finish Up ------Green");
    bool dingbing=false;
#ifdef DINBIAO
 
    String strout="1 shutteruo is "+String(shutterup)+" Finish Up ------Green ";
       printbytcpserver(strout);
    PrintFunc(strout );
        digitalWrite(21, LOW);
        printbytcpserver("please change the fiber in 1 min");
    delay(90000);
    SensorInfo.SensorName=SensorInfo.SensorName+"cali";
    Serial.println("begindown");
        digitalWrite(21, HIGH);
#endif
    
   

    SetShutter(2);
     shutter=OptSnenser(90);
     shutterdown=shutter;
    GetOneDate(shutter);
    memcpy(DownData,DATABUFF,SensorInfo.BandNum*2);
    SetShutter(0);
    GetOneDate(shutter);
    for (int i = 0; i < SensorInfo.BandNum; ++i) {
        int temp=DownData[i]-DATABUFF[i];
        DownData[i]=temp>0?temp:0;

    }
    shutterdown=shutter;
    Serial.println("Down Finish ------Blue");
#ifdef DINBIAO
        String strout1="2 shutterdown is "+String(shutterdown)+" Down Finish ------Blue";
        PrintFunc(strout1 );
        printbytcpserver(strout1);

#endif
}



