//  Copyright (C) 2001-2003, Evolution Robotics, Inc.
//  Any reproduction is strictly prohibited without the explicit
//  written permission of Evolution Robotics, Inc.  All rights reserved.


/**
 * @file    bump_sensor.cpp
 *
 * Test utility for cameras
 */

#pragma warning( disable : 4267 )

//#include <cstdio>

#include <conio.h>
#include <stdlib.h>
#include <signal.h>
#include <math.h>
#include <float.h>
#include <evolution/Resource.hpp>
#include <atltime.h>

// #include "..\\matrix.cpp"

// Globals used by signal handler
#define SENSOR_NAME         "Camera"
#define SENSOR_INTERFACE    "Evolution.ICamera"

// Convenient typedefs for Evolution types
typedef Evolution::Result              Result;
typedef Evolution::ResourceManager     ResourceManager;
typedef Evolution::ResourceConfigParser ResourceConfigParser;
typedef Evolution::IResourceContainer  IResourceContainer;
typedef Evolution::IResource           IResource;
typedef Evolution::ICamera             ICamera;
typedef Evolution::DeviceBusConfig     DeviceBusConfig;
typedef Evolution::DeviceList          DeviceList;
typedef Evolution::String              String;
typedef Evolution::Image               Image;
typedef Evolution::IDriveSystem        IDriveSystem;


typedef Evolution::uchar               uchar;

typedef ICamera Sensor;
typedef std::vector<Sensor*> SensorList;
typedef std::vector<const char*> StringList;

String* sensor_id = new String;

SensorList* sensors = NULL;
StringList* names = NULL;
IResourceContainer* resource_container = NULL;
ResourceManager* g_manager = NULL;
std::auto_ptr<ResourceManager> g_manager_cleanup;

void shutdown_interrupt(int signum)
{
    static bool shutting_down = false;
    signal(signum, shutdown_interrupt);
    if (signum == SIGINT)
    {
        if (shutting_down)
        {
            return;
        }
        shutting_down = true;
    }
}

void exit_handler (void)
{
    /**if (g_manager && (g_manager->is_active ()))
    {
        g_manager->deactivate ();
    }**/
}

Sensor* get_sensor(const char* sensor_id, bool print_error)
{
    // Get sensor interface
    Sensor* sensor = NULL;
    if ( resource_container->obtain_interface (0, sensor_id, 
          SENSOR_INTERFACE, (void**) &sensor) != Evolution::RESULT_SUCCESS )
    {
        if (print_error)
            std::cout << "ERROR: Resource manager failed to obtain " SENSOR_NAME 
                      << " '" << sensor_id << "' (maybe it's not setup as a " 
                      << SENSOR_NAME "?)\n";
        return NULL;
    }

    if (sensor == NULL && print_error)
        std::cout << "ERROR: " SENSOR_NAME " '" << sensor_id << "' not found.\n";

    return sensor;
}

bool get_sensor_list(SensorList* sensors, StringList* names)
{
    std::cout << "Accessing all " SENSOR_NAME "s in system.\n";

    const ResourceConfigParser* r_config = NULL;
    if (resource_container->get_configuration(0, &r_config) != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Could not load Resource Configuration parser.\n";
        return false;
    }

    const ResourceConfigParser::BusList& bus_list = r_config->get_bus_list(); 
    

    for (ResourceConfigParser::BusList::const_iterator bus_iter = bus_list.begin();
         bus_iter != bus_list.end(); ++bus_iter)
    {
        //DeviceBusConfig* device_list = bus_iter.get_data();
        const DeviceBusConfig* device_bus = (*bus_iter);//.devices.begin(); 
        
        //device_list iterator.
        for (DeviceList::const_iterator device_iter = device_bus->devices.begin();
             device_iter != device_bus->devices.end(); ++device_iter) 
        {
            // Get the sensor's name
            
            if ((*device_iter)->get_id(sensor_id) != Evolution::RESULT_SUCCESS)
                continue;

            // Get the sensor interface
            Sensor* sensor = get_sensor(sensor_id->c_str(), 0);
            if (sensor == NULL)
                continue;

            // Its valid-- release the interface for now
            resource_container->release_interface (0, sensor);  

            // Track sensor in list
            const char *name = sensor_id->c_str();
            char *copy = new char[strlen(name)+1];
            strcpy(copy, name);
            sensors->push_back(sensor);
            names->push_back(copy);
            std::cout << "Camera found: " << sensor_id->c_str() << "\n";
        }
    }

    std::cout << "\n";

    return true;
}

void test_sensors(SensorList* sensors, StringList* names,
                  int frames, double quality)
{
    std::cout << "Reading " SENSOR_NAME "s " << frames << " times.\n";

    for (int frame = 1; frame <= frames; frame++)
    {
        std::cout << "Frame " << frame << ": ";

        for (unsigned int i = 0; i < sensors->size() && names->size(); i++)
        {
            std::cout << "  " << (*names)[i] << ": ";

            const Image* image;
            if ((*sensors)[i]->get_image(0, &image) != Evolution::RESULT_SUCCESS)
            {
                std::cout << "Could not get image it took\n";
                continue;
            }

            char filename[80];
            std::sprintf(filename, "%s_%.4i.jpg", (*names)[i], frame);
            if (image->write_file(filename, quality) != Evolution::RESULT_SUCCESS) {
                std::cout << "Failed to write image\n";
                continue;
            } 

            std::cout << "Wrote image\n";
        }

        if (frame < frames) {
            std::cout << "\n";
            Evolution::Platform::millisecond_sleep(500);
        }
    }
}



bool isRed(uchar r, uchar g, uchar b)
{
    return r > 50 && g < 20 && b < 30;
}


void printRow(const Image* i, int row, int width)
{
    uchar r, g, b;

    for (int x = 0; x < width; x++)
    {
        i->get_pixel_rgb(x, row, &r, &g, &b);
        printf("%s", isRed(r, g, b) ? "#" : " ");
    }
}

void printRowChar(const Image* i, int row, int width, int step)
{
    uchar r, g, b;
    printf("Row %d :",row);
    for (int x = 0; x < width; x+=step)
    {
        i->get_pixel_rgb(x, row, &r, &g, &b);
        printf("%s", isRed(r, g, b) ? "#" : " ");
    }
}

void printRow(const Image* i, int row, int width, int step)
{
    uchar r, g, b;
    printf("Row %d :",row);
    for (int x = 0; x < width; x+=step)
    {
        i->get_pixel_rgb(x, row, &r, &g, &b);
        printf("(%d,%d,%d) ", r, g, b);
    }
}

void printImage(const Image* i, int step)
{
    int width  = i->get_width();
    int height = i->get_height();

    for (int y = 0; y < height; y+=step)
    {
        printRow(i, y, width,step);
        printf("\n");
        printRowChar(i, y, width,2);
    }
}

void printImage(const Image* i)
{
    int width  = i->get_width();
    int height = i->get_height();

    for (int y = 0; y < height; y++)
    {
        printRow(i, y, width);
        printf("\n");
    }
}



int get_median(const Image* i, int row)
{
    int w = i->get_width();

    int result;

    int* found_pos = new int[w];
    int  num_found = 0;

    uchar r, g, b;


    for (int x = 0; x < w; x++)
    {
        i->get_pixel_rgb(x, row, &r, &g, &b);
        if (isRed(r, g, b))
            found_pos[num_found++] = x;
    }

    int median = num_found / 2;

    if (num_found % 2)
        result = found_pos[median];
    else
        result = (found_pos[median - 1] + found_pos[median]) / 2;

//  printf("For row %d:\n", row);
//  printRow(i, row, w);

//  printf("Array looks like: ");
//  for (int foo = 0; foo < num_found; foo++)
//      printf("%d ", found_pos[foo]);
//  printf("\n");


    delete[] found_pos;

    if (num_found)
    {
//      printf("result is %d\n", result);
        return result;
    }
    else
    {
//      printf("no result found\n");
        return -1;
    }
}


bool slope(const Image* i, double& slope)
{
    int height = i->get_height();

    int top_line    = 10;
    int bottom_line = height - 10;

    if (slope < -0.1)
        top_line = 100;
    if (slope < -0.2)
        top_line = 160;
        
    int top_1, top_2;
    int bot_1, bot_2;

    do {
        top_line += 4;
        if (top_line >= bottom_line)
            return false;
        top_1 = get_median(i, top_line);
        top_2 = get_median(i, top_line - 1);
    } while (top_1 == -1 || top_2 == -1 || abs(top_1 - top_2) > 20);

    do {
        bottom_line -= 4;
        if (bottom_line <= top_line)
            return false;
        bot_1 = get_median(i, bottom_line);
        bot_2 = get_median(i, bottom_line + 1);
    } while (bot_1 == -1 || bot_2 == -1 || abs(bot_1 - bot_2) > 20);


//  printf("%d %d %d %d\n", top_1, top_2, bot_1, bot_2);

    double dx = ((top_1 + top_2) / 2.0) - ((bot_1 + bot_2) / 2.0);
    double dy = bottom_line - top_line;

    slope = atan2(dx, dy);

//  printf("%.4f\t%.4f\t%.4f\n", dx, dy, slope);

    return true;
}



void accelerate(IDriveSystem* drive, double accel)
{
    drive->move_delta(0, 0, 0, 200, 100, accel*100);
}

IDriveSystem* init_drive()
{
    IDriveSystem* drive;
    std::cout << "Obtaining the drive system interface\n";
    char* interface_name = "drive";
    if (resource_container->obtain_interface (0, interface_name,
                                     IDriveSystem::INTERFACE_ID,
                                     (void**) &drive) != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Resource manager failed to obtain Drived System"
                  << " '" << interface_name << "' (maybe it's not setup as a " 
                  << "Drive system?)\n";
        return NULL;
    }

    return drive;
}



const double BAD_CALIBRATION = 1024.0;

double calibrate_vertical(Sensor* camera)
{
    printf("Ready to calibrate.  Press any key to continue...\n");
    getch();

    double result = BAD_CALIBRATION;

    const Image* image;

    if (camera->get_image(0, &image) != Evolution::RESULT_SUCCESS)
    {
        printf("Couldn't get calibration image!\n");
        return BAD_CALIBRATION;
    }

    if (! slope(image, result))
    {
        printf("Found no red in calibration image!\n");
        return BAD_CALIBRATION;
    }

    printf("Calibration successful!  Offset is %.3f\n", result);


    return result;
}

double calibrate_delay(Sensor* camera)
{
    double result = BAD_CALIBRATION;

    const Image* image;

    int delay_count = 20;

    printf("Calibrating delay with %d pictures...", delay_count);

    DWORD start = GetTickCount();

    for (int i = 0; i < delay_count; i++)
    {
        if (camera->get_image(0, &image) != Evolution::RESULT_SUCCESS)
        {
            printf("Error in getting delay loop image number %d!\n", i);
            return BAD_CALIBRATION;
        }
    }

    DWORD end = GetTickCount();

    DWORD msec = end - start;

    double sec = msec / 1000.0;

    result = sec / (double)delay_count;

    printf("\n...done.  %d pictures taken in %f seconds -- rate of %.3f.\n", delay_count, sec, result);

    return result;
}


void control_loop(IDriveSystem* drive, Sensor* camera, double offset, double delay)
{
    const Image* image;
    double s = 0.0;

    Evolution::Matrix<double> accel(1, 1);
    accel[0][0] = 0;


    Evolution::Matrix<double> A(4, 4);
    A.zeros(4, 4);
    A[0][0] = 3.2868;
    A[0][1] = .7747;
    A[1][0] = 12.6537;
    A[1][1] = 3.2868;
    A[2][2] = 1;
    A[2][3] = 0.46;
    A[3][3] = 1;

    Evolution::Matrix<double> B(4, 1);
    B[0][0] = -0.2333;
    B[1][0] = -1.2912;
    B[2][0] = 0.1058;
    B[3][0] = 0.46;

    Evolution::Matrix<double> C(2, 4);
    C.zeros(2, 4);
    C[0][0] = 1;
    C[1][2] = 1;

    Evolution::Matrix<double> K(1, 4);
    K[0][0] = -14.1838;
    K[0][1] = -3.5052;
    K[0][2] = -.2443;
    K[0][3] = -.5274;


    /*
        Evolution::Matrix<double> A(4, 4);
    A.zeros(4, 4);
    A[0][0] = 1.9094;
    A[0][1] = 0.5924;
    A[1][0] = 4.466;
    A[1][1] = 1.9094;
    A[2][2] = 1;
    A[2][3] = 0.46;
    A[3][3] = 1;

    Evolution::Matrix<double> B(4, 1);
    B[0][0] = -0.0928;
    B[1][0] = -0.4557;
    B[2][0] = 0.1058;
    B[3][0] = 0.46;

    Evolution::Matrix<double> C(2, 4);
    C.zeros(2, 4);
    C[0][0] = 1;
    C[1][2] = 1;

    Evolution::Matrix<double> K(1, 4);
    K[0][0] = -19.8527;
    K[0][1] = -7.2125;
    K[0][2] = -0.4619;
    K[0][3] = -1.0774;
    */
/*

    // Non-Kalman

    Evolution::Matrix<double> L(4, 2);
    L[0][0] = 3.3949;
    L[0][1] = -0.1415;
    L[1][0] = 9.3272;
    L[1][1] = -0.3949;
    L[2][0] = 0.1306;
    L[2][1] = 1.8068;
    L[3][0] = 0.2008;
    L[3][1] = 1.6916;
*/
    // End Non-Kalman

    /*
    // Start Kalman

    Evolution::Matrix<double> L(4, 2);
    L[0][0] = 0.92;
    L[0][1] = -0.0006;
    L[1][0] = 2.5263;
    L[1][1] = -0.0047;
    L[2][0] = -0.0006;
    L[2][1] = 0.3060;
    L[3][0] = -0.0011;
    L[3][1] = 0.1211;

    // End Kalman
*/

    // Kalman 2

    Evolution::Matrix<double> L(4, 2);
    L[0][0] = .9757;
    L[0][1] = -.0003;
    L[1][0] = 3.9436;
    L[1][1] = -.0049;
    L[2][0] = -.0003;
    L[2][1] = .3060;
    L[3][0] = -.0007;
    L[3][1] = .1211;


    Evolution::Matrix<double> status(2, 1);
    status[0][0] = 0;
    status[1][0] = 0;

    Evolution::Matrix<double> tmp1, tmp2, tmp3, tmp4;
    tmp4.zeros(4, 1);

    double a;
    double max = 80.0;

    DWORD start = GetTickCount();
    DWORD image_t = 0;
    DWORD slope_t = 0;
    DWORD drive_t = 0;

    int i = 0;

    while (camera->get_image(0, &image) == Evolution::RESULT_SUCCESS)
    {
        i++;

//      image_t += (GetTickCount()) - start;
//      start = GetTickCount();

//      printf("Image:\t%u\n", GetTickCount() - start);


        if (! slope(image, s))
        {
            printf("Didn't find any red!\n");
            break;
        }

//      slope_t += (GetTickCount()) - start;
//      start = GetTickCount();

//      printf("Slope:\t%u\n", GetTickCount() - start);

        s -= offset;

//      printf("Angle is %.3f\n", s);

        status[0][0] = s;                                  // [0][0] is angle
        status[1][0] = status[1][0] + accel[0][0] * delay; // [1][0] is velocity
/*
        // Without Kalman
        tmp1 = B * accel;
        tmp2 = L * status;
        tmp3 = (A - (L * C)) * tmp4;
        tmp4 = tmp1 + tmp2 + tmp3;

        accel = (K * -1) * tmp4;
*/
        // With Kalman

        tmp1 = A * tmp4;
        tmp2 = B * accel;
        tmp3 = L * (status - (C * tmp4));
        tmp4 = tmp1 + tmp2 + tmp3;

        accel = (K * -1) * tmp4;

        a = accel[0][0] * 100;


//      a = s * 100;

        if (a > max)
            a = max;
        else if (a < -max)
            a = -max;

        printf("Accel: %.3f\n", a);

        drive->move_delta(0, 0, 0, a >= 0 ? 20 : -20, a >= 0 ? 80 : -80, a);
//      drive->move_and_turn(0, a >= 0 ? 40 : -40, a, 0, 0);


//      drive_t += (GetTickCount()) - start;
//      start = GetTickCount();

//      printf("Drive:\t%u\n", GetTickCount() - start);
    }

//  printf("Iterations: %d\nDrive:\t%u\nImage:\t%u\nAngle:\t%u\n", i, drive_t, image_t, slope_t);
}


void cameraTest(SensorList* sensors, StringList* names)
{
    IDriveSystem* drive = init_drive();
    if (drive == NULL)
    {
        Sleep(10000);
        return;
    }
    std::cout << "Reading " SENSOR_NAME "s\n";

    double delay = 0.45; //calibrate_delay((*sensors)[0]);
/*
    if (delay == BAD_CALIBRATION)
    {
        printf("Error in calculating the delay, bailing out.\n");
        return;
    }
*/
    double offset = calibrate_vertical((*sensors)[0]);

    if (offset == BAD_CALIBRATION)
    {
        printf("Error in calibrating the offset, bailing out.\n");
        return;
    }

    printf("Press any key to go...\n");
    getch();

    control_loop(drive, (*sensors)[0], offset, delay);
}

int main (int argc, char** argv)
{
    // Signal Handling
    signal(SIGINT, shutdown_interrupt);
#if !defined(EVOLUTION_PLATFORM_WIN32)
    signal(SIGCHLD, shutdown_interrupt);
    signal(SIGPIPE, shutdown_interrupt);
#endif

    // Exit handling
    std::atexit (exit_handler);

    // Set up logging.
    ERSP_LOG_SET_ROOT_PRIORITY(LOG_INFO);

    //set up containers
    Result result = Evolution::RESULT_SUCCESS;
    ResourceManager* manager;
    manager = new ResourceManager (NULL, result);
    g_manager = manager;
    g_manager_cleanup.reset (g_manager);
    if (result != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Failed to load resource manager.\n";
        return 1;
    }

    if (manager->get_resource_container (0, &resource_container) !=Evolution:: RESULT_SUCCESS)
    {
        std::cout << "ERROR: Failed to get resource container from resource manager.\n";
        return 1;
    }

    if (resource_container == NULL)
    {
        std::cout << "ERROR: Got NULL resource container from resource manager.\n";
        return 1;
    }

    // Create list of sensors to test
    SensorList sensor_list;
    StringList name_list;
    sensors = &sensor_list; // Globalized for shutdown_interrupt()
    names = &name_list;

    double quality = .90;
    int frames = 5;


   if (!get_sensor_list(&sensor_list, &name_list))
         return (1);


    cameraTest(&sensor_list, &name_list);

    printf("\n\nPress any key to quit...\n");
    getch();

    
    /*
    if (argc <= 1)
    {
        std::cout << "Usage: [camera name]... [options].  Use for " SENSOR_NAME "s only.\n"
                  << "Options:\n"
                  << "  --quality <num>    jpeg quality\n"
                  << "  --frames  <num>    number of frames to output\n" 
                  << "\nUsing all cameras found in system at " << quality
                  << "quality for " << frames << " frames\n";

//FIXME: We want to do this even when they specity frames and quality
//but no camera names...  How do we merge that together?
        // Use all sensors in system
        if (!get_sensor_list(&sensor_list, &name_list))
            return (1);
    }

    else
    {
        // Use requested sensors
        for (int i = 1; i < argc; i++)
        {
            if (!strcmp(argv[i], "--frames"))
            {
                // Make sure the array contains another entry
                if (++i < argc)
                    frames = atoi(argv[i]);
                continue;
            }

            if (!strcmp(argv[i], "--quality"))
            {
                // Make sure the array contains another entry
                if (++i < argc)
                    quality = atoi(argv[i]);
                continue;
            }

            Sensor* sensor = get_sensor(argv[i], true);
            if (sensor) {
                const char *name = argv[i];
                char *copy = new char[strlen(name)+1];
                strcpy(copy, name);
                name_list.push_back(copy);
                sensor_list.push_back(sensor);
            }
        }
    }

    if (!sensors->size()) {
        std::cout << "ERROR: No valid " SENSOR_NAME "s found.\n";
        return (1);
    }

    test_sensors(&sensor_list, &name_list, frames, quality);

    std::cout << "Deactivating resources\n";

    // Release allocated interfaces
    if (resource_container && sensors) {
        for (unsigned int i = 0; i < sensors->size(); i++)
            resource_container->release_interface (0, (*sensors)[i]);
        sensors = NULL;
    }

    // Delete string arrays
    if (names) {
        for (unsigned int i = 0; i < names->size(); i++)
            delete [] (*names)[i];
    }
    return (0);

    */
}