#include "main.h"

#include <can/can_client.h>
#include <io/io.h>
#include <mqtt/mqtt_client.h>

struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];


int iBusTimeoutCounter = 0;
/// @brief Increase counter for cycles without received telegram
/// @param iMotorIndex 
void IncBusTimeoutCounter(int iMotorIndex)
{
    if (iBusTimeoutCounter < 2000)
    {
        iBusTimeoutCounter++;
        if (iBusTimeoutCounter >= 2000)
        {
            // long time no telegram received -> motor is not connected anymore
            motctrl[iMotorIndex].nSwitchState = 0;
            motctrl[iMotorIndex].nDriveConnected = 0;
            motctrl[iMotorIndex].nDriveReady = 0;
            Can_SetMotorGear(iMotorIndex, 0);
            MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
        }
    }
}


/// @brief Open socket of CAN interface for the given motor
/// @param iMotorIndex 
/// @param ifacename 
/// @return 
int Can_OpenInterface(int iMotorIndex, const char * ifacename)
{
    // Init control data
    motctrl[iMotorIndex].nDriveConnected = 0;
    motctrl[iMotorIndex].nDriveReady = 0;
    motctrl[iMotorIndex].iActualMotorPowerW = 0;
    motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
    motctrl[iMotorIndex].iMotorPowerRaw = 0;
    motctrl[iMotorIndex].iMotorPowerSteps = 0;
    motctrl[iMotorIndex].nSwitchState = 0;

    mylog(LOG_INFO, "CAN: PWR_MAX_RAW=%d PWR_STEP_COUNT=%d", MOTOR_PWR_MAX_RAW, MOTOR_PWR_STEP_COUNT);
    
    strcpy(intf_data[iMotorIndex].iface_name, ifacename);
    Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
    Can_SetMotorPower(iMotorIndex, 0);

    // first we have to create a socket
    if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
    {
        mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", iMotorIndex);
        return 1;
    }

    // retrieve the interface index
    strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name);
    if (ioctl(intf_data[iMotorIndex].socket, SIOCGIFINDEX, &intf_data[iMotorIndex].ifr) < 0)
    {
        mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", iMotorIndex);
        return 2;
    }

    // bind the socket to the CAN interface
    memset(&intf_data[iMotorIndex].addr, 0, sizeof(intf_data[iMotorIndex].addr));
    intf_data[iMotorIndex].addr.can_family = AF_CAN;
    intf_data[iMotorIndex].addr.can_ifindex = intf_data[iMotorIndex].ifr.ifr_ifindex;
    if (bind(intf_data[iMotorIndex].socket, (struct sockaddr *)&intf_data[iMotorIndex].addr, sizeof(intf_data[iMotorIndex].addr)) < 0)
    {
        mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
        return 3;
    }

    // make socket to nonblocking
    int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
    if (fcntl_flags < 0)
    {
        mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
        return 4;
    }
    fcntl_flags |= O_NONBLOCK;
    if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
    {
        mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
        return 5;
    }

    mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);

    return 0;
}


/// @brief Close socket of CAN interface for the given motor
/// @param iMotorIndex 
void Can_CloseInterface(int iMotorIndex)
{
    if (close(intf_data[iMotorIndex].socket) < 0)
    {
        mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
    }
    else
    {
        mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
    }
}


/// @brief Set gear for the given motor
/// @param iMotorIndex 
/// @param iGear (-1=reverse, 0=neutral, 1=forward)
void Can_SetMotorGear(int iMotorIndex, int iGear)
{
    if ((iGear > 0) && (motctrl[iMotorIndex].nDriveReady != 0))
    {
        if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
        {
            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
            motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
            // motor is switched to forward -> set min. power
            Can_SetMotorPower(iMotorIndex, 1);
        }
        WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear forward.", iMotorIndex);
    } 
    else if ((iGear < 0) && (motctrl[iMotorIndex].nDriveReady != 0))
    {
        if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
        {
            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
            motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
            // motor is switched to reverse -> set min. power
            Can_SetMotorPower(iMotorIndex, 1);
        }
        WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear reverse.", iMotorIndex);
    }
    else 
    {
        if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_NEUTRAL)
        {
            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
            motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
        }
        // motor is switch to neutral -> set power to 0
        Can_SetMotorPower(iMotorIndex, 0);
        WriteOutputPin(GPIO_LED_MOTRUN, LOW);
        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
    } 
}


/// @brief Set power for the given motor
/// @param iMotorIndex 
/// @param iPower (Range: 0..MOTOR_PWR_STEP_COUNT)
void Can_SetMotorPower(int iMotorIndex, int iPower)
{
    if ((motctrl[iMotorIndex].iMotorGear == MOTOR_GEAR_NEUTRAL) || (motctrl[iMotorIndex].nDriveReady == 0))
    {
        // when motor is neutral or not ready set power to 0
        motctrl[iMotorIndex].iMotorPowerSteps = 0;
    }
    else if (iPower <= 1)
    {
        // limit to min. power
        motctrl[iMotorIndex].iMotorPowerSteps = 1;
    }
    else if (iPower >= MOTOR_PWR_STEP_COUNT)
    {
        // limit to max. power
        motctrl[iMotorIndex].iMotorPowerSteps = MOTOR_PWR_STEP_COUNT;
    } 
    else 
    {
        motctrl[iMotorIndex].iMotorPowerSteps = iPower;
    }
    MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerSteps);

    // calc value for telegram
    if (motctrl[iMotorIndex].iMotorPowerSteps <= 0)
    {
        motctrl[iMotorIndex].iMotorPowerRaw = 0;
    }
    else
    {
        motctrl[iMotorIndex].iMotorPowerRaw = (((MOTOR_PWR_MAX_RAW - MOTOR_PWR_MIN_RAW) * motctrl[iMotorIndex].iMotorPowerSteps) / MOTOR_PWR_STEP_COUNT) + MOTOR_PWR_MIN_RAW;
    }

    mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d -> %d",
        iMotorIndex, motctrl[iMotorIndex].iMotorPowerSteps, motctrl[iMotorIndex].iMotorPowerRaw);
}


/// @brief Send CAN protocol for motor gear for the given motor
/// @param iMotorIndex 
void Can_TransmitMotorGear(int iMotorIndex)
{
    // Transmission rate: 100ms
    struct can_frame frame;

    frame.can_id = 0x18F005D0;
    frame.can_id |= CAN_EFF_FLAG;
    frame.can_dlc = 8;
    frame.data[0] = motctrl[iMotorIndex].iMotorGear;
    frame.data[1] = 0xFF;
    frame.data[2] = 0xFF;
    frame.data[3] = 0xFF;
    frame.data[4] = 0xFF;
    frame.data[5] = 0xFF;
    frame.data[6] = 0xFF;
    frame.data[7] = 0xFF;

    if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
    {

    }
}


/// @brief  Send CAN protocol for motor power for the given motor
/// @param iMotorIndex 
void Can_TransmitMotorPower(int iMotorIndex)
{
    // Transmission rate: 50ms
    struct can_frame frame;

    frame.can_id = 0x0CF003D0;
    frame.can_id |= CAN_EFF_FLAG;
    frame.can_dlc = 8;
    frame.data[0] = 0xFF;
    frame.data[1] = motctrl[iMotorIndex].iMotorPowerRaw;   // motor power 0 = 0%, 250 = 100%
    frame.data[2] = 0xFF;
    frame.data[3] = 0xFF;
    frame.data[4] = 0xFF;
    frame.data[5] = 0xFF;
    frame.data[6] = 0xFF;
    frame.data[7] = 0xFF;

    if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
    {

    }
}


/// @brief Read data from CAN interface
/// @param iMotorIndex 
void Can_ReadData(int iMotorIndex)
{
    ssize_t nbytes = 0;
    struct can_frame frame;

    // increment cycle counter
    IncBusTimeoutCounter(iMotorIndex);

    // read one frame
    if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
    {
        canid_t pgn = frame.can_id & 0x00FFFF00;

        switch(pgn)
        {
            case 0x00EF6400:    // "repeat data"
                break;

            case 0x00F00300:    // PGN 61443 "Electronic Engine Controller 2"
                // we have sent this -> ignore
                break;

            case 0x00F00500:    // PGN 61445 "Electronic Transmission Controller 2"
                // we have sent this -> ignore
                break;

            case 0x00FF1300:    // PGN 65299 "Manufacturer PGN"
                // here we find the states of the switches
                Can_Read_Manu_PGN(iMotorIndex, &frame);
                break;

            case 0x00FF1400:    // PGN 65300 "Manufacturer PGN 2"
                // here we find the actual power of the motor
                Can_Read_Manu_PGN2(iMotorIndex, &frame);
                break;
        }
    }
}

/// @brief Read PGN 65299
/// @param frame 
void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
{
    // we received a valid telegram -> set timeout counter to zero
    iBusTimeoutCounter = 0;

    // get switch states
    motctrl[iMotorIndex].nSwitchState = frame->data[4];
    MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);

    // set flags
    motctrl[iMotorIndex].nDriveConnected = 1;       // we received a PGN -> so we are connected
    motctrl[iMotorIndex].nDriveReady = ((motctrl[iMotorIndex].nSwitchState & 0x80) != 0) ? 1 : 0;   // this bit shows if the drive is ready to run
}

/// @brief Read PGN 65300
/// @param frame 
void Can_Read_Manu_PGN2(int iMotorIndex, struct can_frame *frame)
{
    motctrl[iMotorIndex].iActualMotorPowerW = (frame->data[6] << 16) | (frame->data[5] << 8) | frame->data[4];
    MqttClient_Publish_MotorActualPowerW(iMotorIndex, motctrl[iMotorIndex].iActualMotorPowerW);
}