// ------------------------------------------------------------------------
//  J1939 - Receiving Messages
// ------------------------------------------------------------------------
//
// IMPOPRTANT: Depending on the CAN shield used for this programming sample,
//             please make sure you set the proper CS pin in module can.cpp.
//
//  This Arduino program is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.

//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.

#include <stdlib.h>
// #include <SPI.h>

#include "motor.h"
#include "mcp_can.h"
#include "can_ext.h"
// #include "display.h"
// #include "j1939dtc.h"
// #include "j1939.h"
#include "printControl.h"

byte nMyAddr = 0;
#define SA_PREFERRED 128
#define ADDRESSRANGEBOTTOM 129
#define ADDRESSRANGETOP 247

#define GLOBALADDRESS 255
#define NULLADDRESS 254 // NAME Fields Default

// from Thompson Electrak document 5.2.1
#define NAME_IDENTITY_NUMBER 0     // 0xFFFFFF
#define NAME_MANUFACTURER_CODE 547 // 0xFFF
#define NAME_FUNCTION_INSTANCE 0
#define NAME_ECU_INSTANCE 0 // 0x01
#define NAME_FUNCTION 255
#define NAME_RESERVED 0
#define NAME_VEHICLE_SYSTEM 0
#define NAME_VEHICLE_SYSTEM_INSTANCE 0
#define NAME_INDUSTRY_GROUP 0x00 // Global
#define NAME_ARBITRARY_ADDRESS_CAPABLE 0x01

unsigned char ecuNAME[] =
    {
        (byte)(NAME_IDENTITY_NUMBER & 0xFF),
        (byte)((NAME_IDENTITY_NUMBER >> 8) & 0xFF),
        (byte)(((NAME_MANUFACTURER_CODE << 5) & 0xFF) | (NAME_IDENTITY_NUMBER >> 16)),
        (byte)(NAME_MANUFACTURER_CODE >> 3),
        (byte)((NAME_FUNCTION_INSTANCE << 3) | NAME_ECU_INSTANCE),
        (byte)(NAME_FUNCTION),
        (byte)(NAME_VEHICLE_SYSTEM << 1),
        (byte)((NAME_ARBITRARY_ADDRESS_CAPABLE << 7) | (NAME_INDUSTRY_GROUP << 4) | (NAME_VEHICLE_SYSTEM_INSTANCE))};

union u_data
{
  struct
  {
    byte LB;
    byte HB;
  } v;
  unsigned int w;
};

int motorState;

byte blank_response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF};
void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance,
             byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable);

void serviceCAN();
void motorSync();
uint16_t motorErrorFlags(byte motor);

// J1939 j1939;

// Thompson Electrak HD CAN status data
#define motorPGN 0xEF00
#define canAddrClaim 0xEE00

typedef union
{
  byte v[8];
  struct
  {
    unsigned position : 14;
    unsigned current : 9;
    unsigned speed : 5;
    unsigned ve : 2;
    unsigned te : 2;
    unsigned motion : 1;
    unsigned overload : 1;
    unsigned backdrive : 1;
    unsigned parameter : 1;
    unsigned saturation : 1;
    unsigned fatalerror : 1;
    unsigned factory : 18;
    unsigned unused : 8;
  } bits;
} motorStatus_t;

// Thompson Electrak HD CAN control data
typedef union
{
  byte v[8];
  struct
  {
    unsigned position : 14;
    unsigned current_limit : 9;
    unsigned speed : 5;
    unsigned move : 1;
    unsigned factory : 35;
  } bits;
} motorControl_t;

motorStatus_t mStatus[2];
motorControl_t mCtl[2];
bool newData[2];

#define addrM 19 // master can address
#define addrS 20 // slave can address

uint8_t M1Speed;
uint8_t M2Speed;

float motorIntegral = 0;
int16_t maxDelta, minDelta;
uint8_t maxSpeed;
uint32_t startTime, runTime;
uint16_t maxCurrentM, maxCurrentS;

bool bDir = 1; // 1 = extend, 0 - retract
bool bSyncEnable = 1;

uint16_t cycleMax;
uint16_t cycleCount;
uint16_t cycleDelay;

struct
{
  unsigned master : 1;
  unsigned slave : 1;
} motor_start;

uint16_t startMaster, startSlave;
uint16_t lastPosition;

int32_t slaveCurrentBalance = 0; // v984 DES

/*******************  end of variables  ***********************/

// des
uint16_t getMaxCurrent(uint16_t i)
{
  if (i == 0)
    return (maxCurrentM);
  return (maxCurrentS);
}

uint16_t getCurrent(uint16_t i)
{
  if (i == 0)
    return (motorGetCurrent(MASTER));
  return (motorGetCurrent(SLAVE));
}

void setCurrentBalance(int32_t i)
{
  slaveCurrentBalance = i;
}

// ------------------------------------------------------------------------
//  SYSTEM: Setup routine runs on power-up or reset
// ------------------------------------------------------------------------
bool motorInit()
{
  byte i;
  bool canInit = 0;

  //  j1939.init();

  /*
    // Set the NAME
    SetNAME(NAME_IDENTITY_NUMBER,
                 NAME_MANUFACTURER_CODE,
                 NAME_FUNCTION_INSTANCE,
                 NAME_ECU_INSTANCE,
                 NAME_FUNCTION,
                 NAME_VEHICLE_SYSTEM,
                 NAME_VEHICLE_SYSTEM_INSTANCE,
                 NAME_INDUSTRY_GROUP,
                 NAME_ARBITRARY_ADDRESS_CAPABLE);

  */

  // Initialize the CAN controller
  if (canInitialize(CAN_250KBPS) == CAN_OK) // Baud rates defined in mcp_can_dfs.h
  {
    Serial.print("CAN Init OK.\n\r\n\r");
    canInit = 1;
  }
  else
  {
    Serial.print("CAN Init Failed.\n\r");
    return (0);
    delay(1000);
  }

  j1939Transmit(motorPGN, 6, 255, addrM, (byte *)&mCtl[MASTER], 8);
  j1939Transmit(motorPGN, 6, 255, addrS, (byte *)&mCtl[SLAVE], 8);

  mCtl[MASTER].bits.position = 0;
  mCtl[MASTER].bits.current_limit = 250;

  mCtl[SLAVE].bits.position = 0;
  mCtl[SLAVE].bits.current_limit = 250;

  motorState = MOTOR_IDLE;

  return (1);

} // end setup

// ------------------------------------------------------------------------
// Main Loop - Arduino Entry Point
// ------------------------------------------------------------------------
void serviceCan()
{
  // Declarations
  byte nPriority;
  byte nSrcAddr;
  byte nDestAddr;
  byte nData[8];
  int nDataLen;
  long lPGN;
  long pgnReq;
  byte i;

  char sString[80];

  //	j1939.tmrControl();
  // Check for received J1939 messages
  if (j1939Receive(&lPGN, &nPriority, &nSrcAddr, &nDestAddr, nData, &nDataLen) == 0)
  {
    if (lPGN != 0)
    {
      if (nDataLen == 0)
      {
        strcpy(printBuff, "\tNo Data");
        printPrint();
      }
      else
      {
        for (int nIndex = 0; nIndex < nDataLen; nIndex++)
        {
          mStatus[nSrcAddr - addrM].v[nIndex] = nData[nIndex];
          // Serial.print( nData[nIndex]); Serial.print(" ");
        }
        // Serial.println();
      }

      if (nSrcAddr != addrM && nSrcAddr != addrS)
        return;
      switch ((int)lPGN)
      {
      case motorPGN:

        newData[nSrcAddr - addrM] = 1;
        j1939Transmit(motorPGN, 6, 255, nSrcAddr, (byte *)&mCtl[nSrcAddr - addrM], 8);
        break;

      case canAddrClaim:
        break;

      default:
        if (vbCAN)
        {
          sprintf(printBuff, "\r\nUnknown PGN 0x%X %ld", lPGN, lPGN);
          printPrint();
        }
        break;
      } // end 1PGN switch
    }
  } // end if
} // end loop

void motorTask()
{
  serviceCan();

  switch (motorState)
  {
  case MOTOR_IDLE:
    if (motor_start.master || motor_start.slave)
    {
      motorState = MOTOR_START;
    }
    currentDir = RUN_IDLE;
    break;

  case MOTOR_START: //

    // mStatus[0].bits.position = 0;
    // mStatus[1].bits.position = 0;

    masterSpeed = parm[P_MOTOR_SPEED];
    motorSetSpeed(MASTER, masterSpeed); // convert duty cycle to 0 - 31
    motorSetSpeed(SLAVE, masterSpeed);
    motorError = MOTOR_ERROR_NONE;

    motorIntegral = 0.0;
    lastPosition = mStatus[MASTER].bits.position;
    maxDelta = 0;
    minDelta = 0;
    maxSpeed = 0;
    maxCurrentM = 0;
    maxCurrentS = 0;
    startTime = millis();
    Timers[MOTOR_TIMER] = 0;
    printStartOfRun();

    // start the motors
    mCtl[MASTER].bits.move = motor_start.master;
    mCtl[SLAVE].bits.move = motor_start.slave;
    newData[MASTER] = 0;
    newData[SLAVE] = 0;
    motorState = MOTOR_RAMP;
    break;

  case MOTOR_RAMP: // wait here until movement is detected
    if (newData[MASTER] == 1 && newData[SLAVE] == 1)
    {
      if ((mStatus[MASTER].bits.motion == 1) && (mStatus[SLAVE].bits.motion == 1))
      {
        motorState = MOTOR_RUN;
      }
      newData[MASTER] = 0;
      newData[SLAVE] = 0;
    }
    if (Timers[MOTOR_TIMER] > parm[30])
    {
      motorError = MOTOR_ERROR_START_TIMEOUT;
      Serial.println("Start Timeout");
      motorState = MOTOR_STOP;
    }
    break;

  case MOTOR_RUN:
    if (newData[MASTER] == 1 && newData[SLAVE] == 1)
    {
      motorSync();
      if ((mStatus[MASTER].bits.motion == 0) && (mStatus[SLAVE].bits.motion == 0))
      {
        motorState = MOTOR_STOP;
      }
      newData[MASTER] = 0;
      newData[SLAVE] = 0;
      Timers[MOTOR_TIMER] = 0;
      if (mStatus[MASTER].bits.position > lastPosition)
        currentDir = RUN_EXTEND;
      else
        currentDir = RUN_RETRACT;
    }
    if (motorError != MOTOR_ERROR_NONE)
    {
      motorState = MOTOR_STOP;
    }

    if (Timers[MOTOR_TIMER] > parm[31])
    {
      motorError = MOTOR_ERROR_TIMEOUT;
      mStatus[0].bits.position = 9999;
      mStatus[1].bits.position = 9999;
      Serial.println("Run Timeout");
    }
    break;

  case MOTOR_STOP:
    runTime = (millis() - startTime);
    printEndOfRun();
    mCtl[MASTER].bits.move = 0;
    mCtl[SLAVE].bits.move = 0;
    motor_start.master = 0;
    motor_start.slave = 0;
    motorState = MOTOR_IDLE;
    break;

  default:
    Serial.print("Bad State ");
    Serial.println(motorState);
    sprintf(printBuff, "Unknown State %d", motorState);
    printPrint();
    cycleCount = 0;
    motorState = MOTOR_STOP;
    break;
  }
}

// ***************  Start/Stop ***********************
void motorMove(uint16_t position)
{
  mCtl[MASTER].bits.position = position;
  mCtl[SLAVE].bits.position = position;
  motor_start.master = 1;
  motor_start.slave = 1;
  if (position > motorGetPosition(MASTER))
    motorDir = EXTEND;
  else
    motorDir = RETRACT;
}

void motorStart(uint8_t motor)
{
  if (motorState == MOTOR_IDLE)
  {
    motor_start.master = (motor & 1);
    motor_start.slave = (motor & 2) >> 1;
    motorState = MOTOR_START;
  }
}

void motorStop()
{
  if (motorState != MOTOR_IDLE)
  {
    motor_start.master = 0;
    motor_start.slave = 0;
    motorState = MOTOR_STOP;
  }
}

// *************  Speed **************************
void motorSetSpeed(uint8_t motor, uint8_t duty)
{
  uint16_t temp;
  char strTemp[30];
  temp = duty;
  temp *= 32;
  temp /= 100;
  if (temp > 31)
    temp = 31;
  mCtl[motor].bits.speed = temp;
}

uint8_t motorGetSetSpeed(uint8_t motor)
{
  uint16_t duty;
  duty = mCtl[motor].bits.speed;
  duty = (duty * 100) / 32;
  return ((uint8_t)duty);
}
uint8_t motorGetActualSpeed(uint8_t motor)
{
  uint16_t duty;
  duty = mStatus[motor].bits.speed;
  duty = (duty * 100) / 32;
  return ((uint8_t)duty);
}

//************  Position ******************************
void motorSetPosition(uint8_t motor, uint16_t position)
{
  mCtl[motor].bits.position = (position & 0x3FFF);
}
uint16_t motorGetPosition(uint8_t motor)
{
  return (mStatus[motor].bits.position);
}
//***********  Current ********************************
void motorSetCurrentLimit(uint8_t motor, uint16_t limit)
{
  mCtl[motor].bits.current_limit = (limit & 0x1FF);
}
uint16_t motorGetCurrent(uint8_t motor)
{
  return (mStatus[motor].bits.current);
}
//*************  Flags  *********************************
uint8_t motorGetFlags(uint8_t motor, uint8_t flag)
{
  uint8_t data = 0;
  switch (flag)
  {
  case MOTOR_FLAG_VOLTAGE_ERROR:
    data = mStatus[motor].bits.ve;
    break;
  case MOTOR_FLAG_TEMP_ERROR:
    data = mStatus[motor].bits.te;
    break;
  case MOTOR_FLAG_MOTION:
    data = mStatus[motor].bits.motion;
    break;
  case MOTOR_FLAG_OVERLOAD:
    data = mStatus[motor].bits.overload;
    break;
  case MOTOR_FLAG_BACKDRIVE:
    data = mStatus[motor].bits.backdrive;
    break;
  case MOTOR_FLAG_PARAMETER:
    data = mStatus[motor].bits.parameter;
    break;
  case MOTOR_FLAG_SATURATION:
    data = mStatus[motor].bits.saturation;
    break;
  case MOTOR_FLAG_FATAL_ERROR:
    data = mStatus[motor].bits.fatalerror;
    break;
  }
  return (data);
}

/*************************************************************************
  Synchronize motors
  M1 will always run at set speed, M2 will be adjusted to match
    M1 encoder counter

    M2Speed = (delta * P/100) + (I/100 + IG) + M1Speed(masterSpeed)

**************************************************************************/
void motorSync(void)
{

  float delta;
  float newSpeed;
  static byte syncCounter = 0;
  uint16_t slavePosition; // V504

  char sString[30];

  if (++syncCounter < parm[P_MOTOR_ADJ_INTERVAL])
    return;

  syncCounter = 0;
  //  slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET];  //***** V504

  slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET] + slaveCurrentBalance; //***** V983 DES

  if (motorDir == EXTEND)
    delta = mStatus[MASTER].bits.position - slavePosition; // V504
  else
    delta = ((3000 - mStatus[MASTER].bits.position) - (3000 - slavePosition)); // V504

  if (delta == 0)
    return;

  // if the motors get too far apart, shutdown
  if (abs(delta) > parm[P_MAX_DELTA])
  {
    // shut down
    motorError = MOTOR_ERROR_MAX_DELTA;
    /*
    Serial.printf("DELTA Error %d  %d\n",mStatus[MASTER].bits.position,slavePosition);
    Serial.printf("Parm 44 =  %d, Delta = %d\n",parm[44],delta);
        Serial.printf("DELTA Error %d  %d\n",3000 - mStatus[MASTER].bits.position,3000 - slavePosition);
        while(1);
        */
    return;
  }
  M1Speed = masterSpeed;

  if (delta > 0)
    if (delta > maxDelta)
      maxDelta = delta;

  if (delta < 0)
    if (delta < minDelta)
      minDelta = (int16_t)delta;

  if (vbSync)
  {
    dtostrf(delta, 4, 0, sString);
    sprintf(sString, "%s,%u,%u,", sString, motorGetPosition(MASTER), motorGetPosition(SLAVE));
    strcpy(printBuff, sString);
  }

  if (delta < 0) // slave is ahead of master
    motorIntegral -= (float)parm[P_MOTOR_ADJ_I] / 100.0;
  else
    motorIntegral += (float)parm[P_MOTOR_ADJ_I] / 100.0;
  if (vbSync)
  {
    dtostrf(motorIntegral, 6, 2, sString);
    sprintf(sString, "%s,", sString);
    strcat(printBuff, sString);
  }
  newSpeed = (delta * (float)parm[P_MOTOR_ADJ_P]) / 100.0;

  if (vbSync)
  {
    dtostrf(newSpeed, 6, 2, sString);
    sprintf(sString, "%s,", sString);
    strcat(printBuff, sString);
  }

  newSpeed += motorIntegral;

  if (vbSync)
  {
    dtostrf(newSpeed, 6, 2, sString);
    sprintf(sString, "%s,", sString);
    strcat(printBuff, sString);
    // Serial.print(newSpeed,3);
    // Serial.print(",");
  }

  newSpeed += M1Speed;

  if (vbSync)
  {
    dtostrf(newSpeed, 6, 2, sString);
    sprintf(sString, "%s,", sString);
    strcat(printBuff, sString);
  }

  if (newSpeed >= 20 && newSpeed <= 100)
  {
    M2Speed = (byte)newSpeed;
  }
  else if (newSpeed > 100)
  {
    M2Speed = 100;
  }
  else
  {
    M2Speed = 20;
  }
  if (M2Speed > maxSpeed)
    maxSpeed = M2Speed;

  if (bSyncEnable)
    motorSetSpeed(SLAVE, M2Speed);

  if (vbSync)
  {
    sprintf(sString, "%u,%04u,%04u,%04u,%04u",
            M2Speed, motorGetSetSpeed(MASTER), motorGetSetSpeed(SLAVE),
            motorGetCurrent(MASTER), motorGetCurrent(SLAVE));
    strcat(printBuff, sString);
    printPrint();
  }

  if (motorGetCurrent(MASTER) > maxCurrentM)
    maxCurrentM = motorGetCurrent(MASTER);
  if (motorGetCurrent(SLAVE) > maxCurrentS)
    maxCurrentS = motorGetCurrent(SLAVE);
}

void motorJog(uint8_t motor, uint8_t state, uint16_t position)
{
  mCtl[motor].bits.position = position;
  mCtl[motor].bits.move = ~state;

  if (state == 1) // button released
  {
    if (vb)
      printPosition();
  }
}

uint16_t motorErrorFlags(byte motor)
{
  uint16_t flags;

  flags = mStatus[motor].v[4] >> 4;
  flags += (mStatus[motor].v[5] << 4);
  return flags;
}

uint16_t getMotorError(void)
{
  return (motorError);
}

void clearMotorError(void)
{
  motorError = 0;
  mStatus[0].bits.position = 0;
  mStatus[1].bits.position = 0;
}

// SetNAME is not needed

// void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance,
// byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable)
// {
// byte i;
// Serial.print("0X");
// for(i = 0; i < 8; i++)
// Serial.print(ecuNAME[7-i],HEX);

// Serial.println("\r\n");

// ecuNAME[0] = (byte)(v81 & 0xFF);
// ecuNAME[1] = (byte)((v81 >> 8) & 0xFF);
// ecuNAME[2] = (byte)(((nManufacturerCode << 5) & 0xFF) | (v81 >> 16));
// ecuNAME[3] = (byte)(nManufacturerCode >> 3);
// ecuNAME[4] = (byte)((nFunctionInstance << 3) | nECUInstance);
// ecuNAME[5] = (byte)(nFunction);
// ecuNAME[6] = (byte)(nVehicleSystem << 1);
// ecuNAME[7] = (byte)((nArbitraryAddressCapable << 7) | (nIndustryGroup << 4) | (nVehicleSystemInstance));
// Serial.print("0X");
// for(i = 0; i < 8; i++)
// Serial.print(ecuNAME[7-i],HEX);

// Serial.println("\r\n");
// }