#include "pax.h"

//ZFW Entry, fill pax first (pax+bag), rest is cargo
void distribute(paxPayloadData_t* const targetPayload, const FuelData_t* const fuel, const double ZFWTarget, const double CGTarget, const bool isImperial) {
    // Find payload, num pax and extra cargo
    double payload = ZFWTarget - targetPayload->empty - targetPayload->pilot - targetPayload->firstOfficer - targetPayload->engineer -
                     targetPayload->cabinCrewFront - targetPayload->cabinCrewRear - targetPayload->leftAux - targetPayload->rightAux;
    unsigned short numPax = std::max(0.0, std::min((double)MAX_PAX, floor(payload / (PAX_WEIGHT(isImperial) + BAG_WEIGHT(isImperial)))));
    unsigned int cargo = round(payload - numPax * PAX_WEIGHT(isImperial) - numPax * BAG_WEIGHT(isImperial));

    distribute(targetPayload, fuel, numPax, cargo, CGTarget, isImperial);
}

//SimBrief Entry, SB pax count and total cargo
void distribute(paxPayloadData_t* const targetPayload, const FuelData_t* const fuel, unsigned short numPax, unsigned int cargo, const double CGTarget, const bool isImperial) {
    unsigned short _numPax = 0;
    unsigned int count = MAX_PAX;
    // Initial distribution pax + bags
    while (numPax > 0 && count > 0) {
        if (numPax >= 4) {
            if (targetPayload->paxCount.business1 < MAX_BUSINESS_1) {
                targetPayload->paxCount.business1++;
                _numPax++;
            }
            if (targetPayload->paxCount.business2 < MAX_BUSINESS_2) {
                targetPayload->paxCount.business2++;
                _numPax++;
            }
            if (targetPayload->paxCount.economy1 < MAX_ECONOMY_1) {
                targetPayload->paxCount.economy1++;
                _numPax++;
            }
            if (targetPayload->paxCount.economy2 < MAX_ECONOMY_2) {
                targetPayload->paxCount.economy2++;
                _numPax++;
            }
        }
        else if (numPax == 3) {
            if (targetPayload->paxCount.business2 < MAX_BUSINESS_2) {
                targetPayload->paxCount.business2++;
                _numPax++;
            }
            if (targetPayload->paxCount.economy1 < MAX_ECONOMY_1) {
                targetPayload->paxCount.economy1++;
                _numPax++;
            }
            if (targetPayload->paxCount.economy2 < MAX_ECONOMY_2) {
                targetPayload->paxCount.economy2++;
                _numPax++;
            }
        }
        else if (numPax == 2) {
            if (targetPayload->paxCount.economy1 < MAX_ECONOMY_1) {
                targetPayload->paxCount.economy1++;
                _numPax++;
            }
            if (targetPayload->paxCount.economy2 < MAX_ECONOMY_2) {
                targetPayload->paxCount.economy2++;
                _numPax++;
            }
        }
        else if (numPax == 1) {
            if (targetPayload->paxCount.economy2 < MAX_ECONOMY_2) {
                targetPayload->paxCount.economy2++;
                _numPax++;
            }
        }
        numPax -= _numPax;
        targetPayload->paxCount.total += _numPax;

        if (_numPax % 2 == 0) {
            targetPayload->forwardCargo += (_numPax / 2) * BAG_WEIGHT(isImperial);
            targetPayload->rearCargo += (_numPax / 2) * BAG_WEIGHT(isImperial);
        }
        else {
            _numPax--;
            targetPayload->forwardCargo += (_numPax / 2 + 1) * BAG_WEIGHT(isImperial);
            targetPayload->rearCargo += (_numPax / 2) * BAG_WEIGHT(isImperial);
        }

        _numPax = 0;

        count--;
    }
    count = MAX_FRONT_CARGO(isImperial);
    // Initial distibution of remaining cargo
    while (cargo > 0 && count > 0) {
        if (targetPayload->forwardCargo < MAX_FRONT_CARGO(isImperial)) {
            targetPayload->forwardCargo++;
            cargo--;
        }
        if (targetPayload->rearCargo < MAX_REAR_CARGO(isImperial) && cargo > 0) {
            targetPayload->rearCargo++;
            cargo--;
        }

        count--;
    }
    // Refinement pax
    count = MAX_PAX;
    while (count > 0) {
        generatePayload(targetPayload, isImperial);
        calculateCGs(targetPayload, fuel, &targetPayload->ZFWCG, &targetPayload->TOCG, isImperial);

        // in front of target
        if (targetPayload->ZFWCG < CGTarget - CG_TOLERANCE) {
            if (targetPayload->paxCount.business1 > 0) {
                targetPayload->paxCount.business1--;
            }
            else if (targetPayload->paxCount.business2 > 0) {
                targetPayload->paxCount.business2--;
            }
            else if (targetPayload->paxCount.economy1 > 0) {
                targetPayload->paxCount.economy1--;
            }
            else {
                break;
            }

            if (targetPayload->paxCount.economy2 < MAX_ECONOMY_2) {
                targetPayload->paxCount.economy2++;
            }
            else if (targetPayload->paxCount.economy1 < MAX_ECONOMY_1) {
                targetPayload->paxCount.economy1++;
            }
            else if (targetPayload->paxCount.business2 < MAX_BUSINESS_2) {
                targetPayload->paxCount.business2++;
            }
            else {
                targetPayload->paxCount.business1++;
            }
        }
        // behind target
        else if (targetPayload->ZFWCG > CGTarget + CG_TOLERANCE) {
            if (targetPayload->paxCount.economy2 > 0) {
                targetPayload->paxCount.economy2--;
            }
            else if (targetPayload->paxCount.economy1 > 0) {
                targetPayload->paxCount.economy1--;
            }
            else if (targetPayload->paxCount.business2 > 0) {
                targetPayload->paxCount.business2--;
            }
            else {
                break;
            }

            if (targetPayload->paxCount.business1 < MAX_BUSINESS_1) {
                targetPayload->paxCount.business1++;
            }
            else if (targetPayload->paxCount.business2 < MAX_BUSINESS_2) {
                targetPayload->paxCount.business2++;
            }
            else if (targetPayload->paxCount.economy1 < MAX_ECONOMY_1) {
                targetPayload->paxCount.economy1++;
            }
            else {
                targetPayload->paxCount.economy2++;
            }
        }
        else {
            break;
        }

        count--;
    }
    // Refinement cargo
    count = MAX_FRONT_CARGO(isImperial) + MAX_REAR_CARGO(isImperial);
    while (count > 0) {
        generatePayload(targetPayload, isImperial);
        calculateCGs(targetPayload, fuel, &targetPayload->ZFWCG, &targetPayload->TOCG, isImperial);

        // in front of target
        if (targetPayload->ZFWCG < CGTarget - CG_TOLERANCE) {
            if (targetPayload->forwardCargo > 0 && targetPayload->rearCargo < MAX_REAR_CARGO(isImperial)) {
                if (targetPayload->forwardCargo > BAG_WEIGHT(isImperial) &&
                    targetPayload->rearCargo < MAX_FRONT_CARGO(isImperial) - BAG_WEIGHT(isImperial)) {
                    targetPayload->forwardCargo -= BAG_WEIGHT(isImperial);
                    targetPayload->rearCargo += BAG_WEIGHT(isImperial);
                }
                else {
                    targetPayload->forwardCargo--;
                    targetPayload->rearCargo++;
                }
            }
            else {
                break;
            }
        }
        // behind target
        else if (targetPayload->ZFWCG > CGTarget + CG_TOLERANCE) {
            if (targetPayload->rearCargo > 0 && targetPayload->forwardCargo < MAX_FRONT_CARGO(isImperial)) {
                if (targetPayload->rearCargo > BAG_WEIGHT(isImperial) &&
                    targetPayload->forwardCargo < MAX_REAR_CARGO(isImperial) - BAG_WEIGHT(isImperial)) {
                    targetPayload->rearCargo -= BAG_WEIGHT(isImperial);
                    targetPayload->forwardCargo += BAG_WEIGHT(isImperial);
                }
                else {
                    targetPayload->rearCargo--;
                    targetPayload->forwardCargo++;
                }
            }
            else {
                break;
            }
        }
        else {
            break;
        }

        count--;
    }
}

// Updates pax stations with their respective weights
// Used internally and used for Station Entry (pax only, cargo is ste directly)
void generatePayload(paxPayloadData_t* const targetPayload, const bool isImperial) {
    targetPayload->business1Left = targetPayload->business1Center = targetPayload->business1Right = (targetPayload->paxCount.business1 / 3.0) * PAX_WEIGHT(isImperial);
    targetPayload->business2Left = targetPayload->business2Center = targetPayload->business2Right = (targetPayload->paxCount.business2 / 3.0) * PAX_WEIGHT(isImperial);
    targetPayload->economy1Left = targetPayload->economy1Center = targetPayload->economy1Right = (targetPayload->paxCount.economy1 / 3.0) * PAX_WEIGHT(isImperial);
    targetPayload->economy2Left = targetPayload->economy2Center = targetPayload->economy2Right = (targetPayload->paxCount.economy2 / 3.0) * PAX_WEIGHT(isImperial);
}

// Normalise to Pounds
// MANDATORY BEFORE SETTING WEIGHTS
// ENSURE ONLY EVER CALLED ONCE PER SET CYCLE
void normalisePayload(paxPayloadData_t* const targetPayload, const bool isImperial) {
    targetPayload->business1Left = TO_POUNDS(isImperial, targetPayload->business1Left);
    targetPayload->business1Center = TO_POUNDS(isImperial, targetPayload->business1Center);
    targetPayload->business1Right = TO_POUNDS(isImperial, targetPayload->business1Right);
    targetPayload->business2Left = TO_POUNDS(isImperial, targetPayload->business2Left);
    targetPayload->business2Center = TO_POUNDS(isImperial, targetPayload->business2Center);
    targetPayload->business2Right = TO_POUNDS(isImperial, targetPayload->business2Right);
    targetPayload->economy1Left = TO_POUNDS(isImperial, targetPayload->economy1Left);
    targetPayload->economy1Center = TO_POUNDS(isImperial, targetPayload->economy1Center);
    targetPayload->economy1Right = TO_POUNDS(isImperial, targetPayload->economy1Right);
    targetPayload->economy2Left = TO_POUNDS(isImperial, targetPayload->economy2Left);
    targetPayload->economy2Center = TO_POUNDS(isImperial, targetPayload->economy2Center);
    targetPayload->economy2Right = TO_POUNDS(isImperial, targetPayload->economy2Right);
}

void calculateCGs(const paxPayloadData_t* const targetPayload, const FuelData_t* fuel, double* const ZFWCG, double* const TOCG, const bool isImperial) {
    double totalMoment = targetPayload->empty * ARM_EMPTY + targetPayload->pilot * ARM_PILOT + targetPayload->firstOfficer * ARM_FIRST_OFFICER +
                         targetPayload->engineer * ARM_ENGINEER + targetPayload->cabinCrewFront * ARM_PAX_CABIN_CREW_FRONT +
                         targetPayload->business1Left * ARM_PAX_BUSINESS1_LEFT + targetPayload->business1Center * ARM_PAX_BUSINESS1_CENTER +
                         targetPayload->business1Right * ARM_PAX_BUSINESS1_RIGHT +  targetPayload->business2Left * ARM_PAX_BUSINESS2_LEFT +
                         targetPayload->business2Center * ARM_PAX_BUSINESS2_CENTER + targetPayload->business2Right * ARM_PAX_BUSINESS2_RIGHT +
                         targetPayload->economy1Left * ARM_PAX_ECONOMY1_LEFT + targetPayload->economy1Center * ARM_PAX_ECONOMY1_CENTER +
                         targetPayload->economy1Right * ARM_PAX_ECONOMY1_RIGHT + targetPayload->economy2Left * ARM_PAX_ECONOMY2_LEFT +
                         targetPayload->economy2Center * ARM_PAX_ECONOMY2_CENTER + targetPayload->economy2Right * ARM_PAX_ECONOMY2_RIGHT +
                         targetPayload->cabinCrewRear * ARM_PAX_CABIN_CREW_REAR + targetPayload->forwardCargo * ARM_FORWARD_CARGO +
                         targetPayload->rearCargo * ARM_REAR_CARGO + targetPayload->leftAux * ARM_LEFT_AUX + targetPayload->rightAux * ARM_RIGHT_AUX;

    double totalWeight = targetPayload->empty + targetPayload->pilot + targetPayload->firstOfficer + targetPayload->engineer +
                         targetPayload->cabinCrewFront + targetPayload->business1Left + targetPayload->business1Center +
                         targetPayload->business1Right + targetPayload->business2Left + targetPayload->business2Center +
                         targetPayload->business2Right + targetPayload->economy1Left + targetPayload->economy1Center +
                         targetPayload->economy1Right + targetPayload->economy2Left + targetPayload->economy2Center +
                         targetPayload->economy2Right + targetPayload->cabinCrewRear + targetPayload->forwardCargo +
                         targetPayload->rearCargo + targetPayload->leftAux + targetPayload->rightAux;

    *ZFWCG = TO_PERCENT_MAC(totalMoment / totalWeight);

    totalMoment += fuel->main1 * ARM_MAIN1 + fuel->main3 * ARM_MAIN3 + fuel->main2 * ARM_MAIN2 + fuel->upperAux * ARM_UPPER_AUX +
                   fuel->lowerAux * ARM_LOWER_AUX + fuel->main1Tip * ARM_MAIN1_TIP + fuel->main3Tip * ARM_MAIN3_TIP +
                   fuel->tail * ARM_TAIL + fuel->forwardAux1 * ARM_FORWARD_AUX1 + fuel->forwardAux2 * ARM_FORWARD_AUX2;

    totalWeight += fuel->total;

    *TOCG = TO_PERCENT_MAC(totalMoment / totalWeight);
}