Setcontext segmentation fault

I am trying to make threads, switch between them and yield control to each other using getcontext and setcontext.

My code is as follows.

Main.cpp

#include <stdio.h>
#include <iostream>
#include <ucontext.h>
#include <malloc.h>
#include "ult.cpp"

using namespace std;

void MyExampleThread1()
{
    std::cout << "B" << std::endl;
    mythread_yield();
    std::cout << "E" << std::endl;
}

int main()
{
    mythread_init();
    std::cout << "A" << std::endl;
    int thread1 = mythread_fork();
    if (thread1==0)
        MyExampleThread1();
    std::cout << "C" << std::endl;

    // some other commented out code
    return 0;
}

utl.cpp

#include <stdio.h>
#include <iostream>
#include <ucontext.h>
#include <malloc.h>
#include "queue.c"

#define STACK_SIZE SIGSTKSZ*2

using namespace std;
struct Thread
{
    char* stack;
    ucontext_t* ucontext_ptr;
    int id;
};

// Queue which will hold all the threads
int id = 0;
Queue queue = createQueue();

void mythread_init()
{
    // creating the parent thread
    Thread* parent = new Thread;
    parent->id = id;
    parent->ucontext_ptr = new ucontext_t;
    getcontext(parent->ucontext_ptr);
    parent->stack = new char[STACK_SIZE];
    parent->ucontext_ptr->uc_stack.ss_sp = parent->stack;
    parent->ucontext_ptr->uc_stack.ss_size = STACK_SIZE;
    parent->ucontext_ptr->uc_stack.ss_flags = 0;

    // pushing the parent thread into the queue
    queue.push(&queue, *parent);
    std::cout << "Parent Created" << std::endl;
    queue.display(&queue);
    id = id + 1;

}

int mythread_fork()
{
    // creating the child thread
    Thread* child = new Thread;
    child->id = id;
    child->ucontext_ptr = new ucontext_t;
    getcontext(child->ucontext_ptr);

    if (queue.peek(&queue).id == 0){ // parent starting child

        child->stack = new char[STACK_SIZE];
        child->ucontext_ptr->uc_stack.ss_sp = child->stack;
        child->ucontext_ptr->uc_stack.ss_size = STACK_SIZE;
        child->ucontext_ptr->uc_stack.ss_flags = 0;
        // adding the child to the head of the queue
        queue.push(&queue, *child);
        int i, size = queue.size;
        for (i = 0; i < size; i++) {
            if (i > 0){
                queue.push(&queue, queue.pop(&queue));
                }
            }
        std::cout << "Child Created" << std::endl;
        queue.display(&queue);
        id = id + 1;
        setcontext(queue.peek(&queue).ucontext_ptr);
        // parent returning to main
        std::cout << "Parent Returning to Main" << std::endl;
        return child->id;
    }
    else{ // child returning to main
        //setcontext(queue.peek(&queue).ucontext_ptr);
        std::cout << "Child Returning to main" << std::endl;
        return 0;
    }
}
void mythread_yield()
{
    std::cout << "Queue before Yeilding!" << std::endl;
    queue.display(&queue);
    std::cout << "Yeilding!" << std::endl;
    queue.push(&queue, queue.pop(&queue));
    std::cout << "Queue after Yeilding!" << std::endl;
    queue.display(&queue);
    setcontext(queue.peek(&queue).ucontext_ptr);

}

My output is as follows

****Parent Created
Displaying Queue: 1 item(s):0
A
Child Created
Displaying Queue: 2 item(s):1, 0
Child Returning to main
B
Queue before Yeilding!
Displaying Queue: 2 item(s):1, 0
Yeilding!
Queue after Yeilding!
Displaying Queue: 2 item(s):0, 1
Segmentation fault (core dumped)
------------------
(program exited with code: 139)
Press return to continue****

It seems that when I try to move back to the parent thread context, it gives a segmentation fault. Any ideas?

Thanks!