#include "IR_Decoder.h"
#include "IR_Encoder.h"
#include "TimerStatic.h"
#include "MemoryCheck.h"
#include "CarCmdList.h"
/////////////// Pinout ///////////////

#define encForward_PIN    0
#define encBackward_PIN   5

#define LoopOut 12
#define ISR_Out 10

#define TestOut 13

//////////////// Ini /////////////////

#define INFO "IR_FOX TEST"
#define SERIAL_SPEED 115200

//////////////// Var /////////////////
// IR_Encoder encForward(PA5, 42 /* , &decBackward */);

IR_Encoder enc0(PIN_KT8_OUT, 42 /* , &decBackward */);
// IR_Encoder enc1(PA1, 127 /* , &decBackward */);
// IR_Encoder enc2(PA2, 137 /* , &decBackward */);
// IR_Encoder enc3(PA3, 777 /* , &decBackward */);
// IR_Encoder enc10(PA4, 555 /* , &decBackward */);
// IR_Encoder enc11(PC14, 127 /* , &decBackward */);
// IR_Encoder enc12(PC13, 137 /* , &decBackward */);
// IR_Encoder enc13(PA12, 777 /* , &decBackward */);

// IR_Encoder encTree(325, A2);

//////////////////////// Функции прерываний ////////////////////////

void EncoderISR()
{
    IR_Encoder::isr();
}

void decBackwardISR() {
    decBackward.isr();
}

static uint8_t* portOut;
ISR(TIMER2_COMPA_vect) {
    encForward.isr();
    // encBackward.isr();
    // encTree.isr();
    //TODO: Сделать выбор порта 
    *portOut = (*portOut & 0b11111110) |
        (
            encForward.ir_out_virtual << 0U
            // | encBackward.ir_out_virtual << 6U
            // | encTree.ir_out_virtual << 2U
            );
}
/////////////////////////////////////////////////////////////////////
uint8_t data0 [] = { };
uint8_t data1 [] = { 42 };
uint8_t data2 [] = { 42 , 127 };
uint8_t data3 [] = { 42 , 127, 137 };
uint8_t data4 [] = { 42 , 127, 137, 255 };

uint32_t loopTimer;
uint8_t sig = 255;

uint16_t targetAddr = IR_Broadcast;
Timer t1(750, millis, []() {

IR_Decoder dec2(dec2_PIN, 2);
void dec_2_ISR() { dec2.isr(); }

IR_Decoder dec3(dec3_PIN, 3);
void dec_3_ISR() { dec3.isr(); }

IR_Decoder dec4(dec4_PIN, 4);
void dec_4_ISR() { dec4.isr(); }

IR_Decoder dec5(dec5_PIN, 5);
void dec_5_ISR() { dec5.isr(); }

IR_Decoder dec6(dec6_PIN, 6);
void dec_6_ISR() { dec6.isr(); }

IR_Decoder dec7(dec7_PIN, 7);
void dec_7_ISR() { dec7.isr(); }

// IR_Decoder dec8(dec8_PIN, 8);
// void dec_8_ISR() { dec8.isr(); }

// IR_Decoder dec9(dec9_PIN, 9);
// void dec_9_ISR() { dec9.isr(); }

        default:
            break;
    }
    // encBackward.sendData(IR_Broadcast, data2);
    // encTree.sendData(IR_Broadcast, rawData3);
});
Timer t2(500, millis, []() {
    digitalToggle(13);
});
/////////////////////////////////////////////////////////////////////
void setup() {
    IR_Encoder::timerSetup();
    portOut = &PORTB;

// IR_Decoder dec11(dec11_PIN, 11);
// void dec_11_ISR() { dec11.isr(); }

// IR_Decoder dec12(dec12_PIN, 12);
// void dec_12_ISR() { dec12.isr(); }

// IR_Decoder dec13(dec13_PIN, 13);
// void dec_13_ISR() { dec13.isr(); }

/////////////////////////////////////////////////////////////////////
uint8_t data0[] = {};
uint8_t data1[] = {42};
uint8_t data2[] = {42, 127};
uint8_t data3[] = {42, 127, 137};
uint8_t data4[] = {42, 127, 137, 255};

    pinMode(8, OUTPUT);
    pinMode(9, OUTPUT);
    pinMode(11, OUTPUT);
    pinMode(13, OUTPUT);
    pinMode(encForward_PIN, OUTPUT);
    pinMode(encBackward_PIN, OUTPUT);
    pinMode(13, OUTPUT);
    pinMode(12, OUTPUT);

Timer t1(500, millis, []()
         {
             // Serial.println( digitalPinToBitMask(enc0.getPin()), BIN);
            //  enc0.sendData(IR_Broadcast, data4, sizeof(data4));
            //  enc1.sendData(IR_Broadcast, data3, sizeof(data3));
            //  enc2.sendData(IR_Broadcast, data2, sizeof(data2));
            //  enc3.sendData(IR_Broadcast, data1, sizeof(data1));
            //  enc10.sendData(IR_Broadcast, data4, sizeof(data4));
             //  enc11.sendData(IR_Broadcast, data3, sizeof(data3));
             //  enc12.sendData(IR_Broadcast, data2, sizeof(data2));
             //  enc13.sendData(IR_Broadcast, data1, sizeof(data1));

             // Serial.println(sig);

             //  switch (sig)
             //  {
             //  case 0:
             //      encForward.sendData(targetAddr);
             //      break;
             //  case 1:
             //      encForward.sendData(targetAddr, data1, sizeof(data1));
             //      break;
             //  case 2:
             //      encForward.sendData(targetAddr, data2, sizeof(data2));
             //      break;
             //  case 3:
             //      encForward.sendData(targetAddr, data3, sizeof(data3));
             //      break;
             //  case 4:
             //      encForward.sendData(targetAddr, data4, sizeof(data4));
             //      break;

             //  case 10:
             //      encForward.sendData(targetAddr, data0, sizeof(data0), true);
             //      break;
             //  case 11:
             //      encForward.sendData(targetAddr, data1, sizeof(data1), true);
             //      break;
             //  case 12:
             //      encForward.sendData(targetAddr, data2, sizeof(data2), true);
             //      break;
             //  case 13:
             //      encForward.sendData(targetAddr, data3, sizeof(data3), true);
             //      break;
             //  case 14:
             //      encForward.sendData(targetAddr, data4, sizeof(data4), true);
             //      break;

             //  case 20:
             //      encForward.sendBack();
             //      break;
             //  case 21:
             //      encForward.sendBack(data1, sizeof(data1));
             //      break;
             //  case 22:
             //      encForward.sendBack(data2, sizeof(data2));
             //      break;
             //  case 23:
             //      encForward.sendBack(data3, sizeof(data3));
             //      break;
             //  case 24:
             //      encForward.sendBack(data4, sizeof(data4));
             //      break;

             //  case 30:
             //      encForward.sendBackTo(targetAddr);
             //      break;
             //  case 31:
             //      encForward.sendBackTo(targetAddr, data1, sizeof(data1));
             //      break;
             //  case 32:
             //      encForward.sendBackTo(targetAddr, data2, sizeof(data2));
             //      break;
             //  case 33:
             //      encForward.sendBackTo(targetAddr, data3, sizeof(data3));
             //      break;
             //  case 34:
             //      encForward.sendBackTo(targetAddr, data4, sizeof(data4));
             //      break;

             //  case 41:
             //      encForward.sendRequest(targetAddr);
             //      break;
             //  case 42:
             //      encForward.sendAccept(targetAddr);
             //      break;

             //  default:
             //      break;
             //  }
             // encBackward.sendData(IR_Broadcast, data2);
             // encTree.sendData(IR_Broadcast, rawData3);
         });

// Timer t2(50, millis, []()
//          { digitalToggle(LED_BUILTIN); });

Timer signalDetectTimer;
/////////////////////////////////////////////////////////////////////
HardwareTimer IR_Timer(TIM3);

void setup()
{
    IR_Timer.setOverflow(carrierFrec * 2, HERTZ_FORMAT);
    IR_Timer.attachInterrupt(1, EncoderISR);
    NVIC_SetPriority(IRQn_Type::TIM3_IRQn, 0);
    IR_Timer.resume();

    Serial.begin(SERIAL_SPEED);
    Serial.println(F(INFO));

    pinMode(LoopOut, OUTPUT);

    attachInterrupt(0, decForwardISR, CHANGE);  // D2 
    attachInterrupt(1, decBackwardISR, CHANGE); // D3
}


bool testLed = false;
uint32_t testLed_timer;

void loop() {
    // digitalToggle(LoopOut);
    Timer::tick();

    decForward.tick();
    decBackward.tick();

    status(decForward);
    status(decBackward);


    // Serial.println(micros() - loopTimer);
    // loopTimer = micros();
    // delayMicroseconds(120*5);

    if (Serial.available()) {
        uint8_t in = Serial.parseInt();
        switch (in)
        {
        case 100:
            targetAddr = IR_Broadcast;
            break;
        case 101:
            targetAddr = 555;
            break;
        case 102:
            targetAddr = 777;
            break;
        default:
            sig = in;
            break;
        }
    }

    if(testLed && millis() - testLed_timer > 100){
        testLed=false;
        digitalWrite(12, LOW);

    }

}


Timer statusSimpleDelay;
bool statusSimple(IR_Decoder &dec)
{
    bool ret;
    if (ret = dec.gotData.available())
    {
        Serial.print("DEC: ");
        Serial.print(dec.getId());
        Serial.print("   err:  ");
        Serial.print(dec.gotData.getErrorCount());
        Serial.print("\n");
        statusSimpleDelay.delay(100, millis, []()
                                { Serial.print("\n\n\n\n"); });
    }
    return ret;
}

void detectSignal()
{
    // digitalWrite(SignalDetectLed, HIGH);
    // signalDetectTimer.delay(50, millis, []()
    //                         { digitalWrite(SignalDetectLed, LOW); });
}



//test
void status(IR_Decoder& dec) {
    if (dec.gotData.available() && dec.gotData.getAddrFrom() != 42) {

        digitalWrite(12, HIGH);
        testLed = true;
        testLed_timer = millis();

        encForward.sendData(IR_Broadcast, CarCmd::stop); //<<<<<<<<<<<<<<<<<<<<<<<<<<< CMD IS HERE


        String str;
        if (/* dec.gotData.getDataPrt()[1] */ 1)
        {
            str += ("Data on pin ");
            str += (dec.getPin());
            str += "\n";

            uint8_t msg = dec.gotData.getMsgRAW();
            str += ("  MSG: ");
            for (size_t i = 0; i < 8; i++)
            {
                if (i == 3)
                    str += " ";
                str += (msg >> (7 - i)) & 1U;
            }

            str += "\n";

            str += ("  DATA SIZE: ");
            str += (dec.gotData.getDataSize());
            str += "\n";
            str += ("  ADDRESS FROM: ");
            str += (dec.gotData.getAddrFrom());
            str += "\n";
            str += ("  ADDRESS TO: ");
            str += (dec.gotData.getAddrTo());
            str += "\n";
            // str += ("  CRC PACK: "); str += (dec.gotData.getCrcIN()); str += "\n";
            // str += ("  CRC CALC: "); str += (dec.gotData.getCrcCALC()); str += "\n";
            str += "\n";

            for (size_t i = 0; i < min(uint8_t(10), dec.gotData.getDataSize()); i++)
            {
                switch (i)
                {
                    // case 0:
                    //     str += ("  ADDR: ");
                    //     break;
                    // case 1:
                    //     str += ("  CMD: ");
                    //     break;

                default:
                    str += ("  Data[");
                    str += (i);
                    str += ("]: ");
                    break;
                }
                str += (dec.gotData.getDataPrt()[i]);
                str += "\n";
            }

            str += ("\n*******ErrAll: ");
            str += (dec.gotData.getErrorCount());
            str += "\n";
            str += ("**ErrDistance: ");
            str += ((int)(dec.gotData.getErrorHighSignal() - dec.gotData.getErrorLowSignal()));
            str += "\n";

            str += "\n";
        }
        else
        {
            str += ("SELF");
            str += "\n";
            str += "\n";
        }
        // obj->resetAvailable();
        Serial.write(str.c_str());
    }

    if (dec.gotBackData.available())
    {
        detectSignal();
        String str;
        if (/* dec.gotData.getDataPrt()[1] */ 1)
        {
            str += ("BackData on pin ");
            str += (dec.getPin());
            str += "\n";

            uint8_t msg = dec.gotBackData.getMsgRAW();
            str += ("  MSG: ");
            for (size_t i = 0; i < 8; i++)
            {
                if (i == 3)
                    str += " ";
                str += (msg >> (7 - i)) & 1U;
            }

            str += "\n";

            str += ("  DATA SIZE: ");
            str += (dec.gotBackData.getDataSize());
            str += "\n";
            str += ("  ADDRESS FROM: ");
            str += (dec.gotBackData.getAddrFrom());
            str += "\n";
            // str += ("  ADDRESS TO: "); str += (dec.gotBackData.getAddrTo()); str += "\n";
            // str += ("  CRC PACK: "); str += (dec.gotBackData.getCrcIN()); str += "\n";
            // str += ("  CRC CALC: "); str += (dec.gotBackData.getCrcCALC()); str += "\n";
            str += "\n";

            for (size_t i = 0; i < min(uint8_t(10), dec.gotBackData.getDataSize()); i++)
            {
                switch (i)
                {
                    // case 0:
                    //     str += ("  ADDR: ");
                    //     break;
                    // case 1:
                    //     str += ("  CMD: ");
                    //     break;

                default:
                    str += ("  Data[");
                    str += (i);
                    str += ("]: ");
                    break;
                }
                str += (dec.gotBackData.getDataPrt()[i]);
                str += "\n";
            }

            str += ("\n*******ErrAll: ");
            str += (dec.gotBackData.getErrorCount());
            str += "\n";
            str += ("**ErrDistance: ");
            str += ((int)(dec.gotBackData.getErrorHighSignal() - dec.gotBackData.getErrorLowSignal()));
            str += "\n";

            str += "\n";
        }
        else
        {
            str += ("SELF");
            str += "\n";
            str += "\n";
        }
        // obj->resetAvailable();
        Serial.write(str.c_str());
    }

    if (dec.gotAccept.available())
    {
        detectSignal();
        String str;
        if (/* dec.gotData.getDataPrt()[1] */ 1)
        {
            str += ("Accept on pin ");
            str += (dec.getPin());
            str += "\n";

            uint8_t msg = dec.gotAccept.getMsgRAW();
            str += ("  MSG: ");
            for (size_t i = 0; i < 8; i++)
            {
                if (i == 3)
                    str += " ";
                str += (msg >> (7 - i)) & 1U;
            }

            str += "\n";

            // str += ("  DATA SIZE: "); str += (dec.gotAccept.getDataSize()); str += "\n";
            str += ("  ADDRESS FROM: ");
            str += (dec.gotAccept.getAddrFrom());
            str += "\n";
            // str += ("  ADDRESS TO: "); str += (dec.gotAccept.getAddrTo()); str += "\n";
            // str += ("  CRC PACK: "); str += (dec.gotAccept.getCrcIN()); str += "\n";
            // str += ("  CRC CALC: "); str += (dec.gotAccept.getCrcCALC()); str += "\n";
            str += "\n";

            str += ("  Data: ");
            str += (dec.gotAccept.getCustomByte());

            str += ("\n\n*******ErrAll: ");
            str += (dec.gotAccept.getErrorCount());
            str += "\n";
            str += ("**ErrDistance: ");
            str += ((int)(dec.gotAccept.getErrorHighSignal() - dec.gotAccept.getErrorLowSignal()));
            str += "\n";

            str += "\n";
        }
        else
        {
            str += ("SELF");
            str += "\n";
            str += "\n";
        }
        // obj->resetAvailable();
        Serial.write(str.c_str());
    }

    if (dec.gotRequest.available())
    {
        detectSignal();
        String str;
        if (/* dec.gotData.getDataPrt()[1] */ 1)
        {
            str += ("Request on pin ");
            str += (dec.getPin());
            str += "\n";

            uint8_t msg = dec.gotRequest.getMsgRAW();
            str += ("  MSG: ");
            for (size_t i = 0; i < 8; i++)
            {
                if (i == 3)
                    str += " ";
                str += (msg >> (7 - i)) & 1U;
            }

            str += "\n";

            // str += ("  DATA SIZE: "); str += (dec.gotRequest.getDataSize()); str += "\n";
            str += ("  ADDRESS FROM: ");
            str += (dec.gotRequest.getAddrFrom());
            str += "\n";
            str += ("  ADDRESS TO: ");
            str += (dec.gotRequest.getAddrTo());
            str += "\n";
            // str += ("  CRC PACK: "); str += (dec.gotRequest.getCrcIN()); str += "\n";
            // str += ("  CRC CALC: "); str += (dec.gotRequest.getCrcCALC()); str += "\n";
            str += "\n";

            str += ("\n*******ErrAll: ");
            str += (dec.gotRequest.getErrorCount());
            str += "\n";
            str += ("**ErrDistance: ");
            str += ((int)(dec.gotRequest.getErrorHighSignal() - dec.gotRequest.getErrorLowSignal()));
            str += "\n";

            str += "\n";
        }
        else
        {
            str += ("SELF");
            str += "\n";
            str += "\n";
        }
        // obj->resetAvailable();
        Serial.write(str.c_str());
    }
    return false;
}