#include "NmraDcc.h"

#define DCC_PIN 4

#define MOTOR_PWM_CH_A 0
#define MOTOR_PWM_CH_B 1
#define CONFIG_PWM_A_PIN 8
#define CONFIG_PWM_B_PIN 7
#define DEFAULT_DECODER_ADDRESS 3

NmraDcc Dcc;

struct CVPair
{
  uint16_t CV;
  uint8_t Value;
};

CVPair FactoryDefaultCVs[] =
    {
        // The CV Below defines the Short DCC Address
        {CV_MULTIFUNCTION_PRIMARY_ADDRESS, DEFAULT_DECODER_ADDRESS},

        // These two CVs define the Long DCC Address
        {CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB, CALC_MULTIFUNCTION_EXTENDED_ADDRESS_MSB(DEFAULT_DECODER_ADDRESS)},
        {CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB, CALC_MULTIFUNCTION_EXTENDED_ADDRESS_LSB(DEFAULT_DECODER_ADDRESS)},

        // ONLY uncomment 1 CV_29_CONFIG line below as approprate
        //  {CV_29_CONFIG,                                      0}, // Short Address 14 Speed Steps
        {CV_29_CONFIG, CV29_F0_LOCATION}, // Short Address 28/128 Speed Steps
                                          //  {CV_29_CONFIG, CV29_EXT_ADDRESSING | CV29_F0_LOCATION}, // Long  Address 28/128 Speed Steps
};

uint16_t FactoryDefaultCVIndex = 0;
void Motor_init(void)
{
  ledcSetup(MOTOR_PWM_CH_A, 8000, 8);
  ledcAttachPin(CONFIG_PWM_A_PIN, MOTOR_PWM_CH_A);
  ledcWrite(MOTOR_PWM_CH_A, 0);

  ledcSetup(MOTOR_PWM_CH_B, 8000, 8);
  ledcAttachPin(CONFIG_PWM_B_PIN, MOTOR_PWM_CH_B);
  ledcWrite(MOTOR_PWM_CH_B, 0);
};

void MOTOR_Ack(void)
{

  ledcWrite(MOTOR_PWM_CH_B, 0);
  ledcWrite(MOTOR_PWM_CH_A, 255);
  vTaskDelay(200);
  ledcWrite(MOTOR_PWM_CH_A, 0);
};

void notifyCVAck(void)
{
  Serial.println("notifyCVAck! ");
  MOTOR_Ack();
};

void notifyCVResetFactoryDefault()
{
  // Make FactoryDefaultCVIndex non-zero and equal to num CV's to be reset
  // to flag to the loop() function that a reset to Factory Defaults needs to be done
  FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs) / sizeof(CVPair);
};

void notifyDccMsg(DCC_MSG *Msg)
{
  if (Msg->Data[0] != 0xFF)
  {
    Serial.print("notifyDccMsg: ");
    for (uint8_t i = 0; i < Msg->Size; i++)
    {
      Serial.print(Msg->Data[i], HEX);
      Serial.write(' ');
    }
    Serial.println();
  }

  // ESP_LOGI("DCC_MSG","%X",Msg->Data[0]);
};

void check_CV(){
      uint16_t CV_index = sizeof(FactoryDefaultCVs) / sizeof(CVPair);
      for (size_t i = 0; i < CV_index; i++)
      {
        uint16_t CV_key =FactoryDefaultCVs[i].CV;
        Serial.printf("id:%d CV%d: %d\n",i, CV_key, Dcc.getCV(CV_key));
      }
};

void setup()
{
  Serial.begin(115200);
  Dcc.pin(DCC_PIN, 0);
  Dcc.init(MAN_ID_DIY, 10, FLAGS_MY_ADDRESS_ONLY | FLAGS_AUTO_FACTORY_DEFAULT, 0);
  Motor_init();
}

void loop()
{
  Dcc.process();
  vTaskDelay(10);
  // Handle resetting CVs back to Factory Defaults
  if (FactoryDefaultCVIndex && Dcc.isSetCVReady())
  {
    FactoryDefaultCVIndex--; // Decrement first as initially it is the size of the array
    // Serial.printf("id:%d CV%d: %d\n",FactoryDefaultCVIndex, FactoryDefaultCVs[FactoryDefaultCVIndex].CV, FactoryDefaultCVs[FactoryDefaultCVIndex].Value);
    Dcc.setCV(FactoryDefaultCVs[FactoryDefaultCVIndex].CV, FactoryDefaultCVs[FactoryDefaultCVIndex].Value);
  }
  if (Serial.available())
  {
    // Get the new byte and process it.
    switch ((char)Serial.read())
    {
    case '1':
     Serial.println(Dcc.getAddr());
     check_CV();
      break;

    default:
      break;
    }
  }
}