#include "json_handler.h"

#include <QFileDialog>

json_handler::json_handler(QObject *parent)
    :log(parent, QDir::homePath()+"/log.log")
{}

json_handler::~json_handler(){}

bool json_handler::loadTCP(ri::Point& outputPoint, ri::eRobot_TOOL& outputTOOL)
{
    QMessageBox text;
    QString filter = "All File (*.*) ;; JSON File (*.json)";
    QString filename = QFileDialog::getOpenFileName(nullptr,"Load TCP Calibration File (*.JSON)", QDir::homePath(), filter);

    if(!filename.contains(".json", Qt::CaseInsensitive)){
        text.setWindowTitle("WARNING");
        text.setText("NOT Selected TCP Calibration *.JSON File");
        text.exec();
        log<<"Loading TCP Calibration *.JSON file aborted";
        return false;
    }

    try {
        QFile file(filename);
        QString jsonStr;
        file.open(QIODevice::ReadOnly);
        jsonStr = file.readAll();
        file.close();

        QJsonDocument jsonDoc = QJsonDocument::fromJson(jsonStr.toUtf8());
        QJsonObject json = jsonDoc.object();

        QJsonObject::const_iterator it = json.begin();
        QVariantMap result = it->toObject().toVariantMap();

        //TCP -point
        outputPoint.x = result["x"].toDouble();
        outputPoint.y = result["y"].toDouble();
        outputPoint.z = result["z"].toDouble();
        outputPoint.rx = result["rx"].toDouble();
        outputPoint.ry = result["ry"].toDouble();
        outputPoint.rz = result["rz"].toDouble();

        //TCP -tool
        it++;
        result = it->toObject().toVariantMap();
        int tmp = result["TOOL"].toInt();
        try {
            outputTOOL = static_cast<ri::eRobot_TOOL>(tmp);
        } catch (...) {
            log<<"Loaded tool is Not Possible to Define";
            throw std::runtime_error("Loaded tool is Not Possible to Define");
        }
    } catch (...){
        log<<"Loaded TCP Calibration is Not Possible to Load";
        throw std::runtime_error("Loaded TCP Calibration is Not Possible to Load");
    }
    return true;
}

void json_handler::saveTCP(ri::Point inputPoint, ri::eRobot_TOOL inputTOOL)
{
    QString filter = "All File (*.*) ;; JSON File (*.json)";
    QString filename = QFileDialog::getSaveFileName(nullptr, "Save TCP Calibration As", QDir::homePath(), filter);

    if(!filename.contains(".json", Qt::CaseInsensitive)){
            filename.append(".json");
    }

    QJsonObject json2saveObj;

        QJsonObject TCP_pointObj({
            qMakePair(QString("x"),QJsonValue(inputPoint.x)),
            qMakePair(QString("y"),QJsonValue(inputPoint.y)),
            qMakePair(QString("z"),QJsonValue(inputPoint.z)),
            qMakePair(QString("rx"),QJsonValue(inputPoint.rx)),
            qMakePair(QString("ry"),QJsonValue(inputPoint.ry)),
            qMakePair(QString("rz"),QJsonValue(inputPoint.rz))
        });
        json2saveObj.insert(QString("TCP"), QJsonValue(TCP_pointObj));

        json2saveObj.insert(QString("TOOL"),QJsonValue(inputTOOL));

        json2saveObj.insert(QString("Timestamp"), QJsonValue(QDateTime::currentDateTimeUtc().toString()));

        QFile fileWR(filename);
        fileWR.open(QIODevice::WriteOnly);
        QJsonDocument wr(json2saveObj);
        fileWR.write(wr.toJson());
        fileWR.close();
        log<<("*.JSON TCP Calibration file was saved to: "+filename);
        QMessageBox text;
        text.setText("JSON TCP Calibration File was successfully saved");
        text.exec();
}

void json_handler::loadJSON(tcpc::sParameters& parameters, tcpc::sCAM_Parameters& front_camera, tcpc::sCAM_Parameters& side_camera)
{
    QMessageBox text;
    QString filter = "All File (*.*) ;; JSON File (*.json)";
    QString filename = QFileDialog::getOpenFileName(nullptr,"Load Configuration File (*.JSON)", QDir::homePath(), filter);

    if(!filename.contains(".json", Qt::CaseInsensitive)){
        text.setWindowTitle("WARNING");
        text.setText("NOT Selected JSON File");
        text.exec();
        log<<"Loading *.JSON file aborted";
        return;
    }

    QFile file(filename);
    QString jsonStr;
    file.open(QIODevice::ReadOnly);
    jsonStr = file.readAll();
    file.close();

    QJsonDocument jsonDoc = QJsonDocument::fromJson(jsonStr.toUtf8());
    QJsonObject json = jsonDoc.object();

    QJsonObject::const_iterator it = json.begin();
    QVariantMap result = it->toObject().toVariantMap();

    //CV parameters
    QVariantMap cannyObj = result["Canny Edge Detection"].toMap();
    parameters.cv.Canny_threshold.Low = cannyObj["Low Threshold"].toInt();
    parameters.cv.Canny_threshold.High = cannyObj["High Threshold"].toInt();

    QVariantMap paramsObj = result["Other parameters"].toMap();
    parameters.cv.Blur_kernel = paramsObj["Blur kernel"].toInt();
    parameters.cv.Dilate_thickness = paramsObj["Dilate thickness"].toInt();

    //Calibration point
    it++;
    result = it->toObject().toVariantMap();
    QJsonArray  pointF = result["Front view"].toJsonArray();
    QJsonArray  pointS = result["Side view"].toJsonArray();
    parameters.tcpc.calibration_Point.front.x = pointF.at(0).toInt();
    parameters.tcpc.calibration_Point.side.x = pointS.at(0).toInt();
    parameters.tcpc.calibration_Point.front.y = pointF.at(1).toInt();
    parameters.tcpc.calibration_Point.side.y = pointS.at(1).toInt();

    //Camera-Robot calibration
    it++;
    result = it->toObject().toVariantMap();
    QVariantMap CR_pointObj = result["Position"].toMap();
        parameters.camera_robot.position.x = CR_pointObj["x"].toDouble();
        parameters.camera_robot.position.y = CR_pointObj["y"].toDouble();
        parameters.camera_robot.position.z = CR_pointObj["z"].toDouble();
        parameters.camera_robot.position.rx = CR_pointObj["rx"].toDouble();
        parameters.camera_robot.position.ry = CR_pointObj["ry"].toDouble();
        parameters.camera_robot.position.rz = CR_pointObj["rz"].toDouble();

    QVariantMap offset_CRObj = result["Coordinates offset"].toMap();
        parameters.camera_robot.coordinates_offset.x = offset_CRObj["x"].toDouble();
        parameters.camera_robot.coordinates_offset.y = offset_CRObj["y"].toDouble();
        parameters.camera_robot.coordinates_offset.z = offset_CRObj["z"].toDouble();

    //Front Camera
    it++;
    result = it->toObject().toVariantMap();
    front_camera.serial = result["Serial no."].toString().toStdString();
    front_camera.idPf = result["Pixel format"].toString().toStdString();
    front_camera.xmlPath = result["Path *.xml"].toString().toStdString();
    front_camera.exposure = result["Exposure time [us]"].toDouble();

    QJsonArray  K_matF = result["K matrix"].toJsonArray();
    uint8_t et = 0;
    for (int i=0;i<3;i++){
        QJsonArray row = K_matF.at(i).toArray();
        for (int j=0;j<3;j++){
            parameters.stereo_camera.Front_cam.K_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray distF = result["Dist coeffs"].toJsonArray();
    for (int j=0;j<5;j++) {
        parameters.stereo_camera.Front_cam.Dist_coeffs.at<_Float64>(j) = distF.at(j).toDouble();
    }

    QJsonArray RectF = result["Rectification transform"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
    QJsonArray row = RectF.at(i).toArray();
        for (int j=0;j<3;j++) {
            parameters.stereo_camera.Front_cam.Rectification_transform.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray ProjF = result["Projection matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
    QJsonArray row = ProjF.at(i).toArray();
        for (int j=0;j<4;j++) {
            parameters.stereo_camera.Front_cam.Projection_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    //HSV filter
    it++;
    result = it->toObject().toVariantMap();
        QVariantMap frontSideObj = result["Front position"].toMap();
            QVariantMap hueObj = frontSideObj["Hue"].toMap();
                parameters.HSV_filter.front.Hue.Lower = hueObj["Lower boundaries"].toInt();
                parameters.HSV_filter.front.Hue.Upper = hueObj["Upper boundaries"].toInt();
            QVariantMap saturationObj = frontSideObj["Saturation"].toMap();
                parameters.HSV_filter.front.Saturation.Lower = saturationObj["Lower boundaries"].toInt();
                parameters.HSV_filter.front.Saturation.Upper = saturationObj["Upper boundaries"].toInt();
            QVariantMap valueObj = frontSideObj["Value"].toMap();
                parameters.HSV_filter.front.Value.Lower = valueObj["Lower boundaries"].toInt();
                parameters.HSV_filter.front.Value.Upper = valueObj["Upper boundaries"].toInt();
        QVariantMap leftSideObj = result["Side position"].toMap();
            hueObj = leftSideObj["Hue"].toMap();
                parameters.HSV_filter.side.Hue.Lower = hueObj["Lower boundaries"].toInt();
                parameters.HSV_filter.side.Hue.Upper = hueObj["Upper boundaries"].toInt();
            saturationObj = leftSideObj["Saturation"].toMap();
                parameters.HSV_filter.side.Saturation.Lower = saturationObj["Lower boundaries"].toInt();
                parameters.HSV_filter.side.Saturation.Upper = saturationObj["Upper boundaries"].toInt();
            valueObj = leftSideObj["Value"].toMap();
                parameters.HSV_filter.side.Value.Lower = valueObj["Lower boundaries"].toInt();
                parameters.HSV_filter.side.Value.Upper = valueObj["Upper boundaries"].toInt();

    //Robot
    it++;
    result = it->toObject().toVariantMap();
    result["Connection"].toMap();
    QVariantMap connObj = result["Connection"].toMap();
        parameters.Robot.ipv4_Address = connObj["IPv4"].toString().toStdString();
        parameters.Robot.port = connObj["Port"].toInt();
        parameters.Robot.type = result["Type"].toString().toStdString();
    QJsonArray def_pos = result["Default position"].toJsonArray();
        parameters.Robot.default_position.x = def_pos.at(0).toDouble();
        parameters.Robot.default_position.y = def_pos.at(1).toDouble();
        parameters.Robot.default_position.z = def_pos.at(2).toDouble();
        parameters.Robot.default_position.rx = def_pos.at(3).toDouble();
        parameters.Robot.default_position.ry = def_pos.at(4).toDouble();
        parameters.Robot.default_position.rz = def_pos.at(5).toDouble();

    //Side camera
    it++;
    result = it->toObject().toVariantMap();
    side_camera.serial = result["Serial no."].toString().toStdString();
    side_camera.idPf = result["Pixel format"].toString().toStdString();
    side_camera.xmlPath = result["Path *.xml"].toString().toStdString();
    side_camera.exposure = result["Exposure time [us]"].toDouble();

    QJsonArray  K_matS = result["K matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++){
        QJsonArray row = K_matS.at(i).toArray();
        for (int j=0;j<3;j++){
            parameters.stereo_camera.Side_cam.K_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray distS = result["Dist coeffs"].toJsonArray();
    for (int j=0;j<5;j++) {
        parameters.stereo_camera.Side_cam.Dist_coeffs.at<_Float64>(j) = distS.at(j).toDouble();
    }

    QJsonArray RectS = result["Rectification transform"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
    QJsonArray row = RectS.at(i).toArray();
        for (int j=0;j<3;j++) {
            parameters.stereo_camera.Side_cam.Rectification_transform.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray ProjS = result["Projection matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
    QJsonArray row = ProjS.at(i).toArray();
        for (int j=0;j<4;j++) {
            parameters.stereo_camera.Side_cam.Projection_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    //Stereo camera
    it++;
    result = it->toObject().toVariantMap();
    QJsonArray Rot = result["Rotation matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
        QJsonArray row = Rot.at(i).toArray();
        for (int j=0;j<3;j++) {
            parameters.stereo_camera.Rotation_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray Tran = result["Translation vector"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
        QJsonArray row = Tran.at(i).toArray();
        parameters.stereo_camera.Translation_vector.at<_Float64>(et) = row.at(i).toDouble();
        et++;
    }

    QJsonArray Esse = result["Essential matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
        QJsonArray row = Esse.at(i).toArray();
        for (int j=0;j<3;j++) {
            parameters.stereo_camera.Essential_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray Fund = result["Fundamental matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<3;i++) {
        QJsonArray row = Fund.at(i).toArray();
        for (int j=0;j<3;j++) {
            parameters.stereo_camera.Fundamental_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    QJsonArray Qmat = result["Q matrix"].toJsonArray();
    et = 0;
    for (int i=0;i<4;i++) {
        QJsonArray row = Qmat.at(i).toArray();
        for (int j=0;j<4;j++) {
            parameters.stereo_camera.Q_matrix.at<_Float64>(et) = row.at(j).toDouble();
            et++;
        }
    }

    //TCPC default positions
    it++;
    result = it->toObject().toVariantMap();
    QVariantMap def_posObj = result["Default positions"].toMap();
        QVariantMap firstObj = def_posObj["1st position"].toMap();
            parameters.tcpc.default_tcpc_positions.at(0).at(0) = firstObj["j1"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(0).at(1) = firstObj["j2"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(0).at(2) = firstObj["j3"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(0).at(3) = firstObj["j4"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(0).at(4) = firstObj["j5"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(0).at(5) = firstObj["j6"].toDouble();
        QVariantMap secondObj = def_posObj["2nd position"].toMap();
            parameters.tcpc.default_tcpc_positions.at(1).at(0) = secondObj["j1"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(1).at(1) = secondObj["j2"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(1).at(2) = secondObj["j3"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(1).at(3) = secondObj["j4"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(1).at(4) = secondObj["j5"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(1).at(5) = secondObj["j6"].toDouble();
        QVariantMap thirdObj = def_posObj["3rd position"].toMap();
            parameters.tcpc.default_tcpc_positions.at(2).at(0) = thirdObj["j1"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(2).at(1) = thirdObj["j2"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(2).at(2) = thirdObj["j3"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(2).at(3) = thirdObj["j4"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(2).at(4) = thirdObj["j5"].toDouble();
            parameters.tcpc.default_tcpc_positions.at(2).at(5) = thirdObj["j6"].toDouble();

    QVariantMap def_gripObj = result["Default gripper"].toMap();
        parameters.tcpc.default_gripper.x = def_gripObj["x"].toDouble();
        parameters.tcpc.default_gripper.y = def_gripObj["y"].toDouble();
        parameters.tcpc.default_gripper.z = def_gripObj["z"].toDouble();

    parameters.tcpc.precision_px = result["Precision [px]"].toDouble();
    parameters.tcpc.MAX_FINDING_ITER = result["Maximal find iteration"].toInt();
}

void json_handler::saveJSON(tcpc::sParameters parameters, tcpc::sCAM_Parameters front_camera, tcpc::sCAM_Parameters side_camera)
{
    QString path = "";
    QMessageBox::StandardButton appendXML_but;
    appendXML_but = QMessageBox::question(nullptr, "Append *.xml?", "Do you want to append *.xml configuration for the cameras to *.json?",
                                    QMessageBox::Yes|QMessageBox::No);
    if (appendXML_but == QMessageBox::Yes) {
        QString filterXML = "All File (*.*) ;; XML File (*.xml)";
        path = QFileDialog::getOpenFileName(nullptr, "Select a XML file", QDir::homePath(),filterXML);
    }

    QString filter = "All File (*.*) ;; JSON File (*.json)";
    QString filename = QFileDialog::getSaveFileName(nullptr, "Save File As", QDir::homePath(), filter);

    if(!filename.contains(".json", Qt::CaseInsensitive)){
            filename.append(".json");
    }

    QJsonObject json2saveObj;

    QJsonObject cv_paramsObj;

        QJsonObject cannyObjF({
            qMakePair(QString("Low Threshold"),QJsonValue(parameters.cv.Canny_threshold.Low)),
            qMakePair(QString("High Threshold"),QJsonValue(parameters.cv.Canny_threshold.High))
        });
        cv_paramsObj.insert(QString("Canny Edge Detection"), QJsonValue(cannyObjF));

        QJsonObject paramsObjF({
            qMakePair(QString("Blur kernel"),QJsonValue(parameters.cv.Blur_kernel)),
            qMakePair(QString("Dilate thickness"),QJsonValue(parameters.cv.Dilate_thickness))
        });
        cv_paramsObj.insert(QString("Other parameters"), QJsonValue(paramsObjF));

    //isCalibrated();

    QJsonArray K_matF;
        for (int i=0;i<3;i++) {
            QVariantList row;
                for (int j=0;j<3;j++) {
                    row.push_back(parameters.stereo_camera.Front_cam.K_matrix.at<_Float64>(i,j));
                }
            K_matF.push_back(QJsonArray::fromVariantList(row));
        }

    QJsonArray K_matS;
        for (int i=0;i<3;i++) {
            QJsonArray row;
                for (int j=0;j<3;j++) {
                    row.push_back(parameters.stereo_camera.Side_cam.K_matrix.at<_Float64>(i,j));
                }
            K_matS.push_back(row);
        }

    QJsonArray distF;
        for (int j=0;j<5;j++) {
            distF.push_back(parameters.stereo_camera.Front_cam.Dist_coeffs.at<_Float64>(0,j));
        }

    QJsonArray distS;
        for (int j=0;j<5;j++) {
            distS.push_back(parameters.stereo_camera.Side_cam.Dist_coeffs.at<_Float64>(0,j));
        }

    QJsonArray RectF;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            for (int j=0;j<3;j++) {
                row.push_back(parameters.stereo_camera.Front_cam.Rectification_transform.at<_Float64>(i,j));
            }
            RectF.push_back(row);
        }

    QJsonArray RectS;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            for (int j=0;j<4;j++) {
                row.push_back(parameters.stereo_camera.Side_cam.Rectification_transform.at<_Float64>(i,j));
            }
            RectS.push_back(row);
        }

        QJsonArray ProjF;
            for (int i=0;i<3;i++) {
                QJsonArray row;
                for (int j=0;j<4;j++) {
                    row.push_back(parameters.stereo_camera.Front_cam.Projection_matrix.at<_Float64>(i,j));
                }
                ProjF.push_back(row);
            }

        QJsonArray ProjS;
            for (int i=0;i<3;i++) {
                QJsonArray row;
                for (int j=0;j<4;j++) {
                    row.push_back(parameters.stereo_camera.Side_cam.Projection_matrix.at<_Float64>(i,j));
                }
                ProjS.push_back(row);
            }

    QJsonObject camObjF({
        qMakePair(QString("Serial no."),QString(front_camera.serial.c_str())),
        qMakePair(QString("Path *.xml"),QString(path)),
        qMakePair(QString("Pixel format"),QString(front_camera.idPf.c_str())),
        qMakePair(QString("Exposure time [us]"),QJsonValue(front_camera.exposure)),
        qMakePair(QString("Calibrated"),QJsonValue(parameters.stereo_camera.Front_cam.Calibrated)),
        qMakePair(QString("K matrix"),QJsonArray(K_matF)),
        qMakePair(QString("Dist coeffs"),QJsonArray(distF)),
        qMakePair(QString("Rectification transform"),QJsonArray(RectF)),
        qMakePair(QString("Projection matrix"),QJsonArray(ProjF))
    });

    QJsonObject camObjS({
        qMakePair(QString("Serial no."),QString(side_camera.serial.c_str())),
        qMakePair(QString("Path *.xml"),QString(path)),
        qMakePair(QString("Pixel format"),QString(side_camera.idPf.c_str())),
        qMakePair(QString("Exposure time [us]"),QJsonValue(side_camera.exposure)),
        qMakePair(QString("Calibrated"),QJsonValue(parameters.stereo_camera.Side_cam.Calibrated)),
        qMakePair(QString("K matrix"),QJsonArray(K_matS)),
        qMakePair(QString("Dist coeffs"),QJsonArray(distS)),
        qMakePair(QString("Rectification transform"),QJsonArray(RectS)),
        qMakePair(QString("Projection matrix"),QJsonArray(ProjS))
    });

    QJsonArray Rot;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            for (int j=0;j<3;j++) {
                row.push_back(parameters.stereo_camera.Rotation_matrix.at<_Float64>(i,j));
            }
            Rot.push_back(row);
        }

    QJsonArray Tran;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            row.push_back(parameters.stereo_camera.Translation_vector.at<_Float64>(i));
            Tran.push_back(row);
        }

    QJsonArray Esse;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            for (int j=0;j<3;j++) {
                row.push_back(parameters.stereo_camera.Essential_matrix.at<_Float64>(i,j));
            }
            Esse.push_back(row);
        }

    QJsonArray Fund;
        for (int i=0;i<3;i++) {
            QJsonArray row;
            for (int j=0;j<3;j++) {
                row.push_back(parameters.stereo_camera.Fundamental_matrix.at<_Float64>(i,j));
            }
            Fund.push_back(row);
        }

    QJsonArray Qmat;
        for (int i=0;i<4;i++) {
            QJsonArray row;
            for (int j=0;j<4;j++) {
                row.push_back(parameters.stereo_camera.Q_matrix.at<_Float64>(i,j));
            }
            Qmat.push_back(row);
        }

    QJsonObject stereoObj({
        qMakePair(QString("Rotation matrix"),QJsonArray(Rot)),
        qMakePair(QString("Translation vector"),QJsonArray(Tran)),
        qMakePair(QString("Essential matrix"),QJsonArray(Esse)),
        qMakePair(QString("Fundamental matrix"),QJsonArray(Fund)),
        qMakePair(QString("Q matrix"),QJsonArray(Qmat))
    });

    QJsonArray frontPoint;
        frontPoint.push_back(parameters.tcpc.calibration_Point.front.x);
        frontPoint.push_back(parameters.tcpc.calibration_Point.front.y);
    QJsonArray sidePoint;
        sidePoint.push_back(parameters.tcpc.calibration_Point.side.x);
        sidePoint.push_back(parameters.tcpc.calibration_Point.side.y);

    QJsonObject calPointObj({
       qMakePair(QString("Front view"),QJsonArray(frontPoint)),
       qMakePair(QString("Side view"),QJsonArray(sidePoint))
    });

    QJsonObject hsvObj;
        QJsonObject frontSideObj;
            QJsonObject hueF({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.front.Hue.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.front.Hue.Upper))
            });
            QJsonObject saturationF({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.front.Saturation.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.front.Saturation.Upper))
            });
            QJsonObject valueF({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.front.Value.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.front.Value.Upper))
            });
        frontSideObj.insert(QString("Hue"),QJsonValue(hueF));
        frontSideObj.insert(QString("Saturation"),QJsonValue(saturationF));
        frontSideObj.insert(QString("Value"),QJsonValue(valueF));

        hsvObj.insert(QString("Front position"), QJsonValue(frontSideObj));

        QJsonObject leftSideObj;
            QJsonObject hueL({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.side.Hue.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.side.Hue.Upper))
            });
            QJsonObject saturationL({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.side.Saturation.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.side.Saturation.Upper))
            });
            QJsonObject valueL({
                qMakePair(QString("Lower boundaries"),QJsonValue(parameters.HSV_filter.side.Value.Lower)),
                qMakePair(QString("Upper boundaries"),QJsonValue(parameters.HSV_filter.side.Value.Upper))
            });
        leftSideObj.insert(QString("Hue"),QJsonValue(hueL));
        leftSideObj.insert(QString("Saturation"),QJsonValue(saturationL));
        leftSideObj.insert(QString("Value"),QJsonValue(valueL));

        hsvObj.insert(QString("Side position"), QJsonValue(leftSideObj));

    QJsonObject robotObj;
        QJsonObject connection({
            qMakePair(QString("IPv4"),QJsonValue(QString::fromUtf8(parameters.Robot.ipv4_Address.c_str()))),
            qMakePair(QString("Port"),QJsonValue(parameters.Robot.port))
        });
        QJsonArray def_position = {parameters.Robot.default_position.x, parameters.Robot.default_position.y,
                                   parameters.Robot.default_position.z, parameters.Robot.default_position.rx,
                                   parameters.Robot.default_position.ry, parameters.Robot.default_position.rz};

        robotObj.insert(QString("Connection"), QJsonValue(connection));
        robotObj.insert(QString("Type"),QJsonValue(QString::fromUtf8(parameters.Robot.type.c_str())));
        robotObj.insert(QString("Default position"), QJsonValue(def_position));

   QJsonObject camera_robotObj;
        QJsonObject CR_pointObj({
            qMakePair(QString("x"),QJsonValue(parameters.camera_robot.position.x)),
            qMakePair(QString("y"),QJsonValue(parameters.camera_robot.position.y)),
            qMakePair(QString("z"),QJsonValue(parameters.camera_robot.position.z)),
            qMakePair(QString("rx"),QJsonValue(parameters.camera_robot.position.rx)),
            qMakePair(QString("ry"),QJsonValue(parameters.camera_robot.position.ry)),
            qMakePair(QString("rz"),QJsonValue(parameters.camera_robot.position.rz))
        });
        QJsonObject offset_CR_Obj({
            qMakePair(QString("x"),QJsonValue(parameters.camera_robot.coordinates_offset.x)),
            qMakePair(QString("y"),QJsonValue(parameters.camera_robot.coordinates_offset.y)),
            qMakePair(QString("z"),QJsonValue(parameters.camera_robot.coordinates_offset.z))
        });
        camera_robotObj.insert(QString("Position"), QJsonValue(CR_pointObj));
        camera_robotObj.insert(QString("Coordinates offset"), QJsonValue(offset_CR_Obj));

    QJsonObject tcpcObj;
        QJsonObject tcpc_pos_Obj;
            QJsonObject firstObj({
                qMakePair(QString("j1"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(0))),
                qMakePair(QString("j2"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(1))),
                qMakePair(QString("j3"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(2))),
                qMakePair(QString("j4"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(3))),
                qMakePair(QString("j5"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(4))),
                qMakePair(QString("j6"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(0).at(5)))
            });
            QJsonObject secondObj({
                qMakePair(QString("j1"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(0))),
                qMakePair(QString("j2"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(1))),
                qMakePair(QString("j3"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(2))),
                qMakePair(QString("j4"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(3))),
                qMakePair(QString("j5"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(4))),
                qMakePair(QString("j6"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(1).at(5)))
            });
            QJsonObject thirdObj({
                qMakePair(QString("j1"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(0))),
                qMakePair(QString("j2"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(1))),
                qMakePair(QString("j3"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(2))),
                qMakePair(QString("j4"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(3))),
                qMakePair(QString("j5"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(4))),
                qMakePair(QString("j6"),QJsonValue(parameters.tcpc.default_tcpc_positions.at(2).at(5)))
            });
        tcpc_pos_Obj.insert(QString("1st position"), QJsonValue(firstObj));
        tcpc_pos_Obj.insert(QString("2nd position"), QJsonValue(secondObj));
        tcpc_pos_Obj.insert(QString("3rd position"), QJsonValue(thirdObj));

        QJsonObject tcpc_def_gripObj({
            qMakePair(QString("x"),QJsonValue(parameters.tcpc.default_gripper.x)),
            qMakePair(QString("y"),QJsonValue(parameters.tcpc.default_gripper.y)),
            qMakePair(QString("z"),QJsonValue(parameters.tcpc.default_gripper.z))
        });

    tcpcObj.insert(QString("Default positions"), QJsonValue(tcpc_pos_Obj));
    tcpcObj.insert(QString("Precision [px]"), QJsonValue(parameters.tcpc.precision_px));
    tcpcObj.insert(QString("Maximal find iteration"), QJsonValue(parameters.tcpc.MAX_FINDING_ITER));
    tcpcObj.insert(QString("Default gripper"), QJsonValue(tcpc_def_gripObj));

    json2saveObj.insert(QString("Front camera"), QJsonValue(camObjF));
    json2saveObj.insert(QString("Side camera"), QJsonValue(camObjS));
    json2saveObj.insert(QString("Stereo camera"), QJsonValue(stereoObj));
    json2saveObj.insert(QString("CV parameters"), QJsonValue(cv_paramsObj));
    json2saveObj.insert(QString("Calibration point"), QJsonValue(calPointObj));
    json2saveObj.insert(QString("HSV filter"), QJsonValue(hsvObj));
    json2saveObj.insert(QString("Robot interface"), QJsonValue(robotObj));
    json2saveObj.insert(QString("Camera-Robot calibration"), QJsonValue(camera_robotObj));
    json2saveObj.insert(QString("TCPC parameters"), QJsonValue(tcpcObj));

    QFile fileWR(filename);
    fileWR.open(QIODevice::WriteOnly);
    QJsonDocument wr(json2saveObj);
    fileWR.write(wr.toJson());
    fileWR.close();
}
