#include "Armor/Armor.hpp"
#include "AngleSolver/AngleSolver.hpp"
//#include "Serial/sendDataToSTM.h"
#include "opencv2/core/core.hpp"
#include <iostream>

using namespace cv;
using namespace std;

ArmorDetector newDetector;
vector<Point2f> contourPoints;
Point2f centerPoint;
ArmorType type;
AngleSolver newSolver;
float yaw, pitch, r;

int main() {
    newDetector.setTargetNum(1);
    newDetector.loadSVM("../Armor/123svm.xml");
    Mat src;
    string FileName;
    for(int i = 1; i <= 96953; i++) {
        
        //if(!(i % 100)) cout << i << endl;

        FileName = "rmcvdata/roco_train/";
        FileName += i / 10000000 + '0';
        FileName += i / 1000000 % 10 + '0';
        FileName += i / 100000 % 10 + '0';
        FileName += i / 10000 % 10 + '0';
        FileName += i / 1000 % 10 + '0';
        FileName += i / 100 % 10 + '0';
        FileName += i / 10 % 10 + '0';
        FileName += i % 10 + '0';
        FileName += ".jpg";
        
        //FileName = "test.jpg";
        src = imread(samples::findFile(FileName), IMREAD_COLOR);
        if(src.empty()) {
            cout << "file " + FileName + " not found!\n";
            continue;
        }
        
        cout << i << endl;
        cout << endl << endl << "--RED---RED---RED---RED---RED---RED---RED---RED--" << endl;
        newDetector.setEnemyColor(RED);
        newDetector.run(src);
        newDetector.showDebugInfo(1, 1, 1, 1, 1, 1, 1);
        if(newDetector.isFoundArmor())
        {
            newDetector.getTargetInfo(contourPoints, centerPoint, type);
            newSolver.solve(contourPoints, centerPoint, src.size(), yaw, pitch, r);
            cout << "Solved: " << yaw << " " << pitch << " " << r << endl;
            //sendTargetData(pitch, yaw, r, COLOR_RED, 1);
        }
        //else sendTargetData(0.0f, 0.0f, 0.0f, COLOR_RED, 0);
        waitKey(0);
        
        cout << endl << endl << "--BLUE--BLUE--BLUE--BLUE--BLUE--BLUE--BLUE--BLUE-" << endl;
        newDetector.setEnemyColor(BLUE);
        newDetector.run(src);
        newDetector.showDebugInfo(1, 1, 1, 1, 1, 1, 1);
        if(newDetector.isFoundArmor())
        {
            newDetector.getTargetInfo(contourPoints, centerPoint, type);
            newSolver.solve(contourPoints, centerPoint, src.size(), yaw, pitch, r);
            cout << "Solved: " << yaw << " " << pitch << " " << r << endl;
            //sendTargetData(pitch, yaw, r, COLOR_BLUE, 1);
        }
        //else sendTargetData(0.0f, 0.0f, 0.0f, COLOR_BLUE, 0);
        waitKey(0);
    }
    return 0;
}